diff --git a/CMakeLists.txt b/CMakeLists.txt
index cb6ac147..b16389ac 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.6)
find_package(Rock)
rock_init(base 1.0)
-#rock_export_includedir(${PROJECT_SOURCE_DIR}/base ${PROJECT_NAME})
+# rock_export_includedir(${PROJECT_SOURCE_DIR}/base ${PROJECT_NAME})
rock_export_includedir(${PROJECT_SOURCE_DIR}/src base/geometry)
include_directories(BEFORE ${PROJECT_SOURCE_DIR})
@@ -34,7 +34,3 @@ endif()
rock_standard_layout()
-configure_file(base-types.pc.in base-types.pc @ONLY)
-install(DIRECTORY ${CMAKE_SOURCE_DIR}/base DESTINATION include)
-install(FILES ${CMAKE_BINARY_DIR}/base-types.pc DESTINATION lib/pkgconfig)
-
diff --git a/base-types.pc.in b/base-types.pc.in
deleted file mode 100644
index dc18a185..00000000
--- a/base-types.pc.in
+++ /dev/null
@@ -1,11 +0,0 @@
-prefix=@CMAKE_INSTALL_PREFIX@
-exec_prefix=${prefix}
-includedir=${prefix}/include
-
-Name: baseTypes
-Version: 0.1
-Description: Common types for robotics modules
-Libs:
-Requires: eigen3 base-logging
-Cflags: -I${includedir} -I${includedir}/base/backward
-
diff --git a/base/JointLimitRange.hpp b/base/JointLimitRange.hpp
deleted file mode 100644
index 54ae5f2f..00000000
--- a/base/JointLimitRange.hpp
+++ /dev/null
@@ -1,166 +0,0 @@
-#ifndef BASE_JOINT_LIMIT_RANGE_HPP
-#define BASE_JOINT_LIMIT_RANGE_HPP
-
-#include
-#include
-
-namespace base
-{
-
-struct JointLimitRange
-{
- JointState min;
- JointState max;
-
- struct OutOfBounds : public std::runtime_error
- {
- static std::string errorString( std::string name, double min, double max, double value )
- {
- std::ostringstream ss;
- ss << "The " << name << " value " << value
- << " was outside the allowed bounds [" << min << ":" << max << "].";
- return ss.str();
- }
-
- std::string name;
- double min, max, value;
- OutOfBounds()
- : std::runtime_error( std::string() ), min(0), max(0), value(0) {}
- OutOfBounds( std::string name, double min, double max, double value )
- : std::runtime_error( errorString( name, min, max, value ) )
- , name( name )
- , min( min ), max( max ), value( value )
- {
- }
- ~OutOfBounds() throw() {}
- };
-
- /**
- * Checks that the provided JointState is within limits for the values that are valid
- *
- * @throw OutOfBoundsException if limits are exceeded.
- */
- void validate( const JointState &state ) const
- {
- std::pair check = verifyValidity(state);
- if (check.first)
- throw check.second;
- }
-
- /**
- * Checks that the provided JointState is within limits for the values that are valid
- *
- * @return true if the check passes, false otherwise
- */
- bool isValid( const JointState &state ) const
- {
- std::pair check = verifyValidity(state);
- return check.first;
- }
-
- /** Creates a JointLimitRange structure with the position range set to \c
- * min, \c max
- */
- static JointLimitRange Position(double min, double max)
- {
- JointLimitRange result;
- result.min.position = min;
- result.max.position = max;
- return result;
- }
-
- /** Creates a JointLimitRange structure with the speed range set to \c
- * min, \c max
- */
- static JointLimitRange Speed(double min, double max)
- {
- JointLimitRange result;
- result.min.speed = min;
- result.max.speed = max;
- return result;
- }
-
- /** Creates a JointLimitRange structure with the effort range set to \c
- * min, \c max
- */
- static JointLimitRange Effort(double min, double max)
- {
- JointLimitRange result;
- result.min.effort = min;
- result.max.effort = max;
- return result;
- }
-
- /** Creates a JointLimitRange structure with the raw range set to \c
- * min, \c max
- */
- static JointLimitRange Raw(double min, double max)
- {
- JointLimitRange result;
- result.min.raw = min;
- result.max.raw = max;
- return result;
- }
-
- /** Creates a JointLimitRange structure with the acceleration range set to \c
- * min, \c max
- */
- static JointLimitRange Acceleration(double min, double max)
- {
- JointLimitRange result;
- result.min.acceleration = min;
- result.max.acceleration = max;
- return result;
- }
-
-private:
- /** Internal helper method for validate and isValid */
- std::pair verifyValidity( const JointState& state ) const
- {
- using std::make_pair;
- if( state.hasPosition() )
- {
- if( min.hasPosition() && min.position > state.position )
- return make_pair(false, OutOfBounds( "position", min.position, max.position, state.position ));
- if( max.hasPosition() && max.position < state.position )
- return make_pair(false, OutOfBounds( "position", min.position, max.position, state.position ));
- }
-
- if( state.hasSpeed() )
- {
- if( min.hasSpeed() && min.speed > state.speed )
- return make_pair(false, OutOfBounds( "speed", min.speed, max.speed, state.speed ));
- if( max.hasSpeed() && max.speed < state.speed )
- return make_pair(false, OutOfBounds( "speed", min.speed, max.speed, state.speed ));
- }
-
- if( state.hasEffort() )
- {
- if( min.hasEffort() && min.effort > state.effort )
- return make_pair(false, OutOfBounds( "effort", min.effort, max.effort, state.effort ));
- if( max.hasEffort() && max.effort < state.effort )
- return make_pair(false, OutOfBounds( "effort", min.effort, max.effort, state.effort ));
- }
-
- if( state.hasRaw() )
- {
- if( min.hasRaw() && min.raw > state.raw )
- return make_pair(false, OutOfBounds( "raw", min.raw, max.raw, state.raw ));
- if( max.hasRaw() && max.raw < state.raw )
- return make_pair(false, OutOfBounds( "raw", min.raw, max.raw, state.raw ));
- }
-
- if( state.hasAcceleration() )
- {
- if( min.hasAcceleration() && min.acceleration > state.acceleration )
- return make_pair(false, OutOfBounds( "acceleration", min.acceleration, max.acceleration, state.acceleration ));
- if( max.hasAcceleration() && max.acceleration < state.acceleration )
- return make_pair(false, OutOfBounds( "acceleration", min.acceleration, max.acceleration, state.acceleration ));
- }
- return make_pair(true, OutOfBounds());
- }
-};
-
-}
-
-#endif
diff --git a/base/JointLimits.hpp b/base/JointLimits.hpp
deleted file mode 100644
index ee1d4c40..00000000
--- a/base/JointLimits.hpp
+++ /dev/null
@@ -1,51 +0,0 @@
-#ifndef BASE_JOINT_LIMITS_HPP
-#define BASE_JOINT_LIMITS_HPP
-
-#include
-#include
-#include
-
-namespace base
-{
- struct JointLimits : public NamedVector
- {
- bool isValid( const base::samples::Joints& joints ) const
- {
- if (joints.hasNames())
- {
- for( size_t i=0; i
-#include
-#include
-#include
-#include
-
-namespace base
-{
-
-/**
- * defines the frame transformation for a joint,
- * and the rotation axis
- */
-struct JointTransform
-{
- std::string sourceFrame;
- std::string targetFrame;
- base::Vector3d rotationAxis;
-};
-
-/**
- * Vector of JointTranformations with added functionality
- * to fill a vector of RigidBodyStates given a jointsState sample
- */
-struct JointTransformVector : public base::NamedVector
-{
- /**
- * will fill the rbs structure with the information from the joints and
- * the JointTransform configuration.
- */
- void setRigidBodyStates( const base::samples::Joints& joints, std::vector& rbs ) const
- {
- if (joints.names.empty()) {
- throw std::runtime_error("base::JointTransformVector::"
- "setRigidBodyStates(): the vector "
- "'joints.names()' is empty");
- }
-
- rbs.resize( joints.size() );
- for( size_t i=0; i template)"
-#include
-#include
-#include
-#include
-#include
-
-namespace base
-{
- namespace odometry = ::odometry;
-}
-
-#endif
diff --git a/base/Temperature.hpp b/base/Temperature.hpp
deleted file mode 100644
index 37c580e9..00000000
--- a/base/Temperature.hpp
+++ /dev/null
@@ -1,175 +0,0 @@
-#ifndef __BASE_TEMPERATURE_HH__
-#define __BASE_TEMPERATURE_HH__
-
-#include
-#include // std::abs
-#include
-
-namespace base
-{
-
-/**
- * This class represents a temperature, and can be used instead of double for
- * convenience. The class has a canonical representation of the temperature in
- * kelvin.
- */
-class Temperature
-{
-public:
-
- /**
- * temperature in kelvins
- *
- *
- * @note don't use this value directly. It's only public to allow this class
- * to be used as an interface type.
- */
- double kelvin;
-
- /**
- * default constructor, which will leave the temperature uninitialized.
- */
- Temperature() : kelvin(base::unknown()) {}
-
-protected:
- explicit Temperature( double kelvin ) : kelvin(kelvin)
- {
-
- }
-
-
-public:
- /**
- * static conversion from kelvin to celsius
- * @param kelvin temperature in kelvin
- * @result temperature in celsius
- */
- static inline double kelvin2Celsius( double kelvin )
- {
- return kelvin - 273.15;
- }
-
- /**
- * static conversion from celsius to kelvin
- * @param celsius temperature in celsius
- * @result temperature in kelvin
- */
- static inline double celsius2Kelvin( double celsius )
- {
- return celsius + 273.15;
- }
-
- /**
- * use this method to get temperature from Kelvin.
- * @return representation of the given temperature.
- * @param kelvin - temperature in Kelvin.
- */
- static inline Temperature fromKelvin( double kelvin )
- {
- return Temperature( kelvin );
- }
-
- /**
- * use this method to get temperature from Celsius
- * @return representation of the given temperature.
- * @param celsius - temperature in celsius.
- */
- static inline Temperature fromCelsius( double celsius )
- {
- return Temperature( celsius + 273.15);
- }
-
- /**
- * @return canonical value of the temperature in kelvin
- */
- double inline getKelvin() const
- {
- return kelvin;
- }
-
- /**
- * @return canonical value of the temperature in celsius
- */
- double inline getCelsius() const
- {
- return kelvin - 273.15;
- }
-
- /**
- * @return true if the temperature is insight the given interval
- */
- bool inline isInRange(const Temperature &left_limit,const Temperature &right_limit) const;
-
- /**
- * compare two temperatures for approximate equality
- * @param other - temperature to compare
- * @param prec - precision interval in kelvin
- * @return true if temperature is approximately equal
- */
- bool inline isApprox( Temperature other, double prec = 1e-5 ) const
- {
- return std::abs( other.kelvin - kelvin ) < prec;
- }
-
- void operator=(const Temperature &other)
- {
- kelvin = other.kelvin;
- }
-
- inline bool operator==(const Temperature &other ) const
- {
- return this->kelvin == other.kelvin;
- }
-
- inline bool operator<(const Temperature &other ) const
- {
- return this->kelvin < other.kelvin;
- }
-
- inline bool operator>(const Temperature &other ) const
- {
- return this->kelvin > other.kelvin;
- }
-
-};
-
-static inline Temperature operator+( Temperature a, Temperature b )
-{
- return Temperature::fromKelvin( a.getKelvin() + b.getKelvin() );
-}
-
-static inline Temperature operator-( Temperature a, Temperature b )
-{
- return Temperature::fromKelvin( a.getKelvin() - b.getKelvin() );
-}
-
-static inline Temperature operator*( Temperature a, double b )
-{
- return Temperature::fromKelvin( a.getKelvin() * b );
-}
-
-static inline Temperature operator*( double a, Temperature b )
-{
- return Temperature::fromKelvin( a * b.getKelvin() );
-}
-
-
-
-static inline std::ostream& operator << (std::ostream& os, Temperature temperature)
-{
- os << temperature.getCelsius() << boost::format("[%3.1f celsius]");
- return os;
-}
-
-bool Temperature::isInRange(const Temperature &left_limit, const Temperature &right_limit) const
-{
- if((right_limit-left_limit).kelvin < 0)
- return !isInRange(right_limit,left_limit);
- if((*this -left_limit).getKelvin() >= 0 && (right_limit -*this).getKelvin() >= 0)
- return true;
- return false;
-}
-
-}
-
-#endif
diff --git a/base/Time.hpp b/base/Time.hpp
deleted file mode 100644
index 7e984181..00000000
--- a/base/Time.hpp
+++ /dev/null
@@ -1,252 +0,0 @@
-#ifndef BASE_TIME_H__
-#define BASE_TIME_H__
-
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-
-namespace base
-{
- struct Time
- {
- private:
- explicit Time(int64_t _microseconds)
- : microseconds(_microseconds) { }
- public:
- int64_t microseconds;
-
- static const int UsecPerSec = 1000000LL;
-
- enum Resolution { Seconds = 1, Milliseconds = 1000, Microseconds = 1000000 };
-
- Time()
- : microseconds(0) {}
-
-
- public:
- /** Returns the current time */
- static Time now() {
- timeval t;
- gettimeofday(&t, 0);
- return Time(static_cast(t.tv_sec) * UsecPerSec + t.tv_usec);
- }
-
- bool operator < (Time const& ts) const
- { return microseconds < ts.microseconds; }
- bool operator > (Time const& ts) const
- { return microseconds > ts.microseconds; }
- bool operator == (Time const& ts) const
- { return microseconds == ts.microseconds; }
- bool operator != (Time const& ts) const
- { return !(*this == ts); }
- bool operator >= (Time const& ts) const
- { return !(*this < ts); }
- bool operator <= (Time const& ts) const
- { return !(*this > ts); }
- Time operator - (Time const& ts) const
- { return Time(microseconds - ts.microseconds); }
- Time operator + (Time const& ts) const
- { return Time(microseconds + ts.microseconds); }
- Time operator / (int divider) const
- { return Time(microseconds / divider); }
- Time operator * (double factor) const
- { return Time(microseconds * factor); }
-
- /** True if this time is zero */
- bool isNull() const { return microseconds == 0; }
-
- /** Converts this time as a timeval object */
- timeval toTimeval() const
- {
- timeval tv = { static_cast(microseconds / UsecPerSec), static_cast(microseconds % UsecPerSec) };
- return tv;
- }
-
- /** Convert time into a string
- * \param resolution Resolution which the string should present
- * \param mainFormat Main format to use -- this is passed to strftime and appended by ':' plus the
- * below seconds resolution if requested by the resolution argument
- **/
- std::string toString(base::Time::Resolution resolution = Microseconds, const std::string& mainFormat = "%Y%m%d-%H:%M:%S") const
- {
- struct timeval tv = toTimeval();
- int uSecs = tv.tv_usec;
-
- time_t when = tv.tv_sec;
- struct tm *tm = localtime(&when);
-
- char time[50];
- strftime(time,50, mainFormat.c_str(), tm);
-
- char buffer[57];
- switch(resolution)
- {
- case Seconds:
- return std::string(time);
- case Milliseconds:
- sprintf(buffer,"%s:%03d", time, (int) (uSecs/1000.0));
- break;
- case Microseconds:
- sprintf(buffer,"%s:%06d", time, uSecs);
- break;
- default:
- throw std::invalid_argument(
- "base::Time::toString(): invalid "
- "value in switch-statement");
- }
-
- return std::string(buffer);
- }
-
- /** Returns this time as a fractional number of seconds */
- double toSeconds() const
- { return static_cast(microseconds) / UsecPerSec; }
- /** Returns this time as an integer number of milliseconds (thus dropping the microseconds) */
- int64_t toMilliseconds() const
- { return microseconds / 1000; }
- /** Returns this time as an integer number of microseconds */
- int64_t toMicroseconds() const
- { return microseconds; }
- static Time fromMicroseconds(uint64_t value)
- { return Time(value); }
- static Time fromMilliseconds(uint64_t value)
- { return Time(value * 1000); }
- static Time fromSeconds(int64_t value)
- { return Time(value * UsecPerSec); }
- static Time fromSeconds(int value)
- { return Time(static_cast(value) * UsecPerSec); }
- static Time fromSeconds(int64_t value, int microseconds)
- { return Time(value * UsecPerSec + static_cast(microseconds)); }
- static Time fromSeconds(double value)
- {
- int64_t seconds = value;
- return Time(seconds * UsecPerSec + static_cast(round((value - seconds) * UsecPerSec)));
- }
-
-
- /**
- * \brief Create time from int Time Values.
- * Creates a time object from the time values (year, month, day ...) given as integer values. This function can be used
- * when the time values are only available as seperated values in numerical form.
- * \param year The year as integer value. (should be 4 digits)
- * \param month The month of the year (1..12).
- * \param day Day of the month (1..31).
- * \param hour The hour of the day (since midnight 0..23).
- * \param minute The minutes after the hour (0..59).
- * \param seconds The seconds after the minute (0..59)
- * \param millis Milliseconds after the last second (0..999)
- * \param micros Microseconds additional to the milliseconds (0..999)
- * \returns a Time object generated from the parameters.
- *
- */
- static Time fromTimeValues(int year, int month, int day, int hour, int minute, int seconds, int millis, int micros)
- {
-
- struct tm timeobj;
- timeobj.tm_year = year - 1900;
- timeobj.tm_mon = month - 1;
- timeobj.tm_mday = day;
- timeobj.tm_hour = hour;
- timeobj.tm_min = minute;
- timeobj.tm_sec = seconds;
- timeobj.tm_isdst = -1;
-
- time_t tTime;
- tTime = mktime(&timeobj);
-
- int64_t timeVal = static_cast(tTime);
-
- timeVal = timeVal * UsecPerSec;
- timeVal += millis * 1000 + micros;
-
-
- return Time(timeVal);
-
- }
-
-
- /**
- * Create a time object from an input string, by default all parameters are set to convert the string returned
- * by toString back to a Time object.
- * \param stringTime String describing the time
- * \param resolution Set to a resolution higher than Secs if a (non-standard) msec or usec field is present, i.e. the non standard field is separated by ':'
- * \param mainFormat valid format for strptime, e.g."%Y%m%d-%H:%M:%S" which the given time string has
- * \throws std::runtime_error on failure such as a mismatching format
- */
- static Time fromString(const std::string& stringTime, Resolution resolution = Microseconds, const std::string& mainFormat = "%Y%m%d-%H:%M:%S")
- {
- std::string mainTime = stringTime;
- int32_t usecs = 0;
- if(resolution > Seconds)
- {
- size_t pos = stringTime.find_last_of(':');
- std::string usecsString = stringTime.substr(pos+1);
- size_t usecsStringLength = usecsString.size();
- if( (usecsStringLength == 3 || usecsStringLength == 6) && !(usecsStringLength == 3 && resolution > Milliseconds))
- {
- // string matches resolutions
- } else
- {
- throw std::runtime_error("base::Time::fromString failed - resolution does not match provided Time-String");
- }
-
- switch(resolution)
- {
- case Milliseconds:
- sscanf(usecsString.c_str(), "%03d", &usecs);
- usecs = usecs*1000;
- break;
- case Microseconds:
- sscanf(usecsString.c_str(), "%06d", &usecs);
- break;
- case Seconds:
- throw std::invalid_argument(
- "base::Time::fromString(); "
- "'Seconds' is an invalid case "
- "here");
- default:
- throw std::invalid_argument("base::Time::fromString(): "
- "invalid value in "
- "switch-statement");
- }
- }
-
- struct tm tm;
- if(NULL == strptime(mainTime.c_str(), mainFormat.c_str(), &tm))
- {
- throw std::runtime_error("base::Time::fromString failed- Time-String '" + mainTime + "' did not match the given format '" + mainFormat +"'");
- }
- // " ... not set by strptime(); tells mktime() to determine
- // whether daylight saving time is in effect ..."
- // (http://pubs.opengroup.org/onlinepubs/007904975/functions/strptime.html)
-
- tm.tm_isdst = -1;
- time_t time = mktime(&tm);
-
- return Time(static_cast(time)*UsecPerSec + static_cast(usecs));
- }
-
- };
-
- inline std::ostream& operator << (std::ostream& io, base::Time const& time)
- {
- const int64_t microsecs = time.toMicroseconds();
-
- io << (microsecs / 1000000)
- << std::setfill('0')
- << "." << std::setw(3) << (std::llabs(microsecs) / 1000) % 1000
- << "." << std::setw(3) << (std::llabs(microsecs) % 1000)
- << std::setfill(' ');
-
- return io;
- }
-}
-
-
-#endif
diff --git a/base/TimeMark.hpp b/base/TimeMark.hpp
deleted file mode 100644
index ae05987b..00000000
--- a/base/TimeMark.hpp
+++ /dev/null
@@ -1,39 +0,0 @@
-#ifndef BASE_TIMEMARK_H__
-#define BASE_TIMEMARK_H__
-
-#include
-#include
-
-namespace base {
-
-struct TimeMark
-{
- std::string label;
- Time mark;
- clock_t clock;
-
- TimeMark(const std::string& label) : label(label), mark( Time::now() ), clock( ::clock() ) {};
-
- /** Return the time that has passed since the recorded time and now */
- Time passed() const
- {
- return (Time::now() - mark);
- }
-
- clock_t cycles() const
- {
- return ::clock() - clock;
- }
-};
-
-
-inline std::ostream &operator<<(std::ostream &stream, const TimeMark &ob)
-{
- stream << ob.cycles() << "cyc (" << ob.passed() << "s) since " << ob.label;
- return stream;
-}
-
-} // namespace base
-
-
-#endif
diff --git a/base/TransformWithCovariance.hpp b/base/TransformWithCovariance.hpp
deleted file mode 100644
index 2a0ce417..00000000
--- a/base/TransformWithCovariance.hpp
+++ /dev/null
@@ -1,413 +0,0 @@
-#ifndef __BASE_TRANSFORM_WITH_COVARIANCE_HPP__
-#define __BASE_TRANSFORM_WITH_COVARIANCE_HPP__
-
-#include // std::setprecision
-
-#include
-#include
-#include
-#include
-
-#include
-#include
-#include
-
-namespace base {
-
- /**
- * Class which represents a 3D Transformation with associated uncertainty information.
- *
- * The uncertainty is represented as a 6x6 matrix, which is the covariance
- * matrix of the [r t] representation of the error. Here r is the rotation orientation
- * part expressed as a scaled axis of orientation, and t the translational
- * component.
- *
- * The uncertainty information is optional. The hasValidCovariance() method can
- * be used to see if uncertainty information is associated with the class.
- */
- class TransformWithCovariance
- {
-
- public:
- typedef base::Matrix6d Covariance;
-
- public:
- /** The transformation is represented 6D vector [translation orientation]
- * Here orientation is the rotational part expressed as a quaternion
- * orientation, and t the translational component.
- */
- base::Position translation;
-
- base::Quaterniond orientation;
-
- /** The uncertainty is represented as a 6x6 matrix, which is the covariance
- * matrix of the [translation orientation] representation of the error.
- */
- Covariance cov;
-
- public:
-
- TransformWithCovariance() : translation(base::Position::Zero()),
- orientation(base::Quaterniond::Identity()) {this->invalidateCovariance();}
-
- explicit TransformWithCovariance( const base::Affine3d& trans)
- {this->setTransform(trans); this->invalidateCovariance();};
-
- TransformWithCovariance( const base::Affine3d& trans, const Covariance& cov )
- {this->setTransform(trans); this->cov = cov;};
-
- TransformWithCovariance(const base::Position& translation, const base::Quaterniond& orientation) :
- translation(translation), orientation(orientation) {this->invalidateCovariance();};
-
- TransformWithCovariance(const base::Position& translation, const base::Quaterniond& orientation, const Covariance& cov ) :
- translation(translation), orientation(orientation), cov(cov){};
-
- static TransformWithCovariance Identity()
- {
- return TransformWithCovariance();
- };
-
-
- /** Default std::cout function
- */
- friend std::ostream & operator<<(std::ostream &out, const TransformWithCovariance& trans);
-
- /** performs a composition of this transform with the transform given.
- * The result is another transform with result = this * trans
- */
- TransformWithCovariance composition( const TransformWithCovariance& trans ) const
- {
- return this->operator*( trans );
- };
-
- /** performs an inverse composition of two transformations.
- * The result is such that result * trans = this. Note that this is different from
- * calling result = this * inv(trans), in the way the uncertainties are handled.
- */
- TransformWithCovariance compositionInv( const TransformWithCovariance& trans ) const
- {
- const TransformWithCovariance &tf(*this);
- const TransformWithCovariance &t1(trans);
- base::Position p2(tf.translation + (tf.orientation * t1.inverse().translation));
- Eigen::Quaterniond q2( tf.orientation * t1.orientation.inverse());
-
- // short path if there is no uncertainty
- if( !t1.hasValidCovariance() && !tf.hasValidCovariance() )
- return TransformWithCovariance(p2, q2);
-
- // convert the orientations of the respective transforms into quaternions
- // in order to inverse the covariances, we need to get both the t1 and t2=[r2 p2] transformations
- // based on the composition tf = t2 * t1
- Eigen::Quaterniond q1( t1.orientation );
- Eigen::Quaterniond q( q2 * q1 );
-
- // initialize resulting covariance
- Eigen::Matrix cov = Eigen::Matrix::Zero();
-
- Eigen::Matrix J1;
- J1 << q2.toRotationMatrix(), Eigen::Matrix3d::Zero(),
- Eigen::Matrix3d::Zero(), dr2r1_by_r1(q, q1, q2);
-
- Eigen::Matrix J2;
- J2 << Eigen::Matrix3d::Identity(), drx_by_dr(q2, t1.translation),
- Eigen::Matrix3d::Zero(), dr2r1_by_r2(q, q1, q2);
-
- cov = J2.inverse() * ( tf.getCovariance() - J1 * t1.getCovariance() * J1.transpose() ) * J2.transpose().inverse();
-
- // and return the resulting uncertainty transform
- return TransformWithCovariance( p2, q2, cov );
- };
-
- /** Same as compositionInv, just that the result is such that trans * result = this.
- * Note that this is different from calling result = inv(trans) * this,
- * in the way the uncertainties are handled.
- */
- TransformWithCovariance preCompositionInv( const TransformWithCovariance& trans ) const
- {
- const TransformWithCovariance &tf(*this);
- const TransformWithCovariance &t2(trans);
- base::Position p1(t2.inverse().translation + (t2.orientation.inverse() * tf.translation));
- Eigen::Quaterniond q1(t2.orientation.inverse() * tf.orientation);
-
- // short path if there is no uncertainty
- if( !t2.hasValidCovariance() && !tf.hasValidCovariance() )
- return TransformWithCovariance( p1, q1 );
-
- // convert the orientations of the respective transforms into quaternions
- // in order to inverse the covariances, we need to get both the t1=[p1 q1] and t2 transformations
- // based on the composition tf = t2 * t1
- Eigen::Quaterniond q2(t2.orientation);
- Eigen::Quaterniond q( q2 * q1 );
-
- // initialize resulting covariance
- Eigen::Matrix cov = Eigen::Matrix::Zero();
-
- Eigen::Matrix J1;
- J1 << t2.getTransform().linear(), Eigen::Matrix3d::Zero(),
- Eigen::Matrix3d::Zero(), dr2r1_by_r1(q, q1, q2);
-
- Eigen::Matrix J2;
- J2 << Eigen::Matrix3d::Identity(), drx_by_dr(q2, p1),
- Eigen::Matrix3d::Zero(), dr2r1_by_r2(q, q1, q2);
-
- cov = J1.inverse() * ( tf.getCovariance() - J2 * t2.getCovariance() * J2.transpose() ) * J1.transpose().inverse();
-
- // and return the resulting uncertainty transform
- return TransformWithCovariance( p1, q1, cov );
- };
-
- /** alias for the composition of two transforms
- */
- TransformWithCovariance operator*( const TransformWithCovariance& trans ) const
- {
- const TransformWithCovariance &t2(*this);
- const TransformWithCovariance &t1(trans);
-
- const base::Quaterniond t(t2.orientation * t1.orientation);
- const base::Position p(t2.translation + (t2.orientation * t1.translation));
-
- // short path if there is no uncertainty
- if( !t1.hasValidCovariance() && !t2.hasValidCovariance() )
- {
- return TransformWithCovariance(p, t);
- }
-
- // convert the orientations of the respective transforms into quaternions
- const Eigen::Quaterniond q1( t1.orientation ), q2( t2.orientation );
- const Eigen::Quaterniond q( t2.orientation * t1.orientation );
-
- // initialize resulting covariance
- Eigen::Matrix cov = Eigen::Matrix::Zero();
-
- // calculate the Jacobians (this is what all the above functions are for)
- // and add to the resulting covariance
- if( t1.hasValidCovariance() )
- {
- Eigen::Matrix J1;
- J1 << t2.getTransform().linear(), Eigen::Matrix3d::Zero(),
- Eigen::Matrix3d::Zero(), dr2r1_by_r1(q, q1, q2);
-
- cov += J1*t1.getCovariance()*J1.transpose();
- }
-
- if( t2.hasValidCovariance() )
- {
- Eigen::Matrix J2;
- J2 << Eigen::Matrix3d::Identity(), drx_by_dr(q2, t1.translation),
- Eigen::Matrix3d::Zero(), dr2r1_by_r2(q, q1, q2);
-
- cov += J2*t2.getCovariance()*J2.transpose();
- }
-
- // and return the resulting uncertainty transform
- return TransformWithCovariance(p, t, cov);
- };
-
- TransformWithCovariance inverse() const
- {
- // short path if there is no uncertainty
- if( !hasValidCovariance() )
- return TransformWithCovariance(static_cast(-(this->orientation.inverse() * this->translation)), this->orientation.inverse());
-
- Eigen::Quaterniond q(this->orientation);
- Eigen::Vector3d t(this->translation);
- Eigen::Matrix J;
- J << q.toRotationMatrix().transpose(), drx_by_dr( q.inverse(), t ),
- Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Identity();
-
- return TransformWithCovariance(static_cast(-(this->orientation.inverse() * this->translation)),
- this->orientation.inverse(),
- J*this->getCovariance()*J.transpose());
- };
-
- const Covariance& getCovariance() const { return this->cov; }
- void setCovariance( const Covariance& cov ) { this->cov = cov; }
-
- const base::Matrix3d getTranslationCov() const { return this->cov.topLeftCorner<3,3>(); }
- void setTranslationCov(const base::Matrix3d& cov) { this->cov.topLeftCorner<3,3>() = cov; }
-
- const base::Matrix3d getOrientationCov() const { return this->cov.bottomRightCorner<3,3>(); }
- void setOrientationCov(const base::Matrix3d& cov) { this->cov.bottomRightCorner<3,3>() = cov; }
-
- const base::Affine3d getTransform() const
- {
- base::Affine3d trans (this->orientation);
- trans.translation() = this->translation;
- return trans;
- }
- void setTransform( const base::Affine3d& trans )
- {
- this->translation = trans.translation();
- this->orientation = base::Quaterniond(trans.rotation());
- }
-
- const base::Orientation getOrientation() const
- {
- return base::Orientation(this->orientation);
- }
-
- void setOrientation(const base::Orientation & q)
- {
- this->orientation = base::Quaterniond(q);
- }
-
- bool hasValidTransform() const
- {
- return !translation.hasNaN() && !orientation.coeffs().hasNaN();
- }
-
- void invalidateTransform()
- {
- translation = base::Position::Ones() * base::NaN();
- orientation.coeffs() = base::Vector4d::Ones() * base::NaN();
- }
-
- /** @warning This method is computationally expensive. Use with care! */
- bool hasValidCovariance() const { return !cov.hasNaN(); }
- void invalidateCovariance()
- {
- cov = Covariance::Ones() * base::NaN();
- }
-
- protected:
- // The uncertainty transformations are implemented according to:
- // Pennec X, Thirion JP. A framework for uncertainty and validation of 3-D
- // registration methods based on points and frames. International Journal of
- // Computer Vion. 1997;25(3):203–229. Available at:
- // http://www.springerlink.com/index/JJ25N2Q23T402682.pdf.
-
- static Eigen::Quaterniond r_to_q( const Eigen::Vector3d& r )
- {
- double theta = r.norm();
- if( fabs(theta) > 1e-5 )
- return Eigen::Quaterniond( base::AngleAxisd( theta, r/theta ) );
- else
- return Eigen::Quaterniond::Identity();
- }
-
- static Eigen::Vector3d q_to_r( const Eigen::Quaterniond& q )
- {
- base::AngleAxisd aa( q );
- return aa.axis() * aa.angle();
- }
-
- static inline double sign( double v )
- {
- return v > 0.0 ? 1.0 : -1.0;
- }
-
- static Eigen::Matrix skew_symmetric( const Eigen::Vector3d& r )
- {
- Eigen::Matrix3d res;
- res << 0, -r.z(), r.y(),
- r.z(), 0, -r.x(),
- -r.y(), r.x(), 0;
- return res;
- }
-
- static Eigen::Matrix dq_by_dr( const Eigen::Quaterniond& q )
- {
- const Eigen::Vector3d r( q_to_r( q ) );
-
- const double theta = r.norm();
- const double kappa = 0.5 - theta*theta / 48.0; // approx. see Paper
- const double lambda = 1.0/24.0*(1.0-theta*theta/40.0); // approx.
- Eigen::Matrix res;
- res << - q.vec().transpose()/2.0,
- kappa * Eigen::Matrix3d::Identity() - lambda * r * r.transpose();
-
- return res;
- }
-
- static Eigen::Matrix dr_by_dq( const Eigen::Quaterniond& q )
- {
- const Eigen::Vector3d r( q_to_r( q ) );
- const double mu = q.vec().norm();
- const double tau = 2.0 * sign( q.w() ) * ( 1.0 + mu*mu/6.0 ); // approx
- const double nu = -2.0 * sign( q.w() ) * ( 2.0/3.0 + mu*mu/5.0 ); // approx
-
- Eigen::Matrix res;
- res << -2*q.vec(), tau * Eigen::Matrix3d::Identity() + nu * q.vec() * q.vec().transpose();
-
- return res;
- }
-
- static Eigen::Matrix dq2q1_by_dq1( const Eigen::Quaterniond& q2 )
- {
- Eigen::Matrix res;
- res << 0, -q2.vec().transpose(),
- q2.vec(), skew_symmetric( q2.vec() );
- return Eigen::Matrix::Identity() * q2.w() + res;
- }
-
- static Eigen::Matrix dq2q1_by_dq2( const Eigen::Quaterniond& q1 )
- {
- Eigen::Matrix res;
- res << 0, -q1.vec().transpose(),
- q1.vec(), -skew_symmetric( q1.vec() );
- return Eigen::Matrix::Identity() * q1.w() + res;
- }
-
- static Eigen::Matrix dr2r1_by_r1( const Eigen::Quaterniond& q, const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2 )
- {
- return Eigen::Matrix3d(
- dr_by_dq( q )
- * dq2q1_by_dq1( q2 )
- * dq_by_dr( q1 ) );
- }
-
- static Eigen::Matrix dr2r1_by_r2( const Eigen::Quaterniond& q, const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2 )
- {
- return Eigen::Matrix3d(
- dr_by_dq( q )
- * dq2q1_by_dq2( q1 )
- * dq_by_dr( q2 ) );
- }
-
- static Eigen::Matrix drx_by_dr( const Eigen::Quaterniond& q, const Eigen::Vector3d& x )
- {
- const Eigen::Vector3d r( q_to_r( q ) );
- const double theta = r.norm();
- const double alpha = 1.0 - theta*theta/6.0;
- const double beta = 0.5 - theta*theta/24.0;
- const double gamma = 1.0 / 3.0 - theta*theta/30.0;
- const double delta = -1.0 / 12.0 + theta*theta/180.0;
-
- return Eigen::Matrix3d(
- -skew_symmetric(x)*(gamma*r*r.transpose()
- - beta*skew_symmetric(r)+alpha*Eigen::Matrix3d::Identity())
- -skew_symmetric(r)*skew_symmetric(x)*(delta*r*r.transpose()
- + 2.0*beta*Eigen::Matrix3d::Identity()) );
- }
- };
-
- /** Default std::cout function
- */
- inline std::ostream & operator<<(std::ostream &out, const TransformWithCovariance& trans)
- {
- /** cout the 6D pose vector (translation and scaled axis orientation) with its associated covariance matrix **/
- base::Vector3d scaled_axis;
- base::AngleAxisd angle_axis (trans.orientation);
- scaled_axis = angle_axis.axis() * angle_axis.angle();
- for (register unsigned short i=0; i // std::setprecision
-
-#include
-#include
-#include
-#include
-
-#include
-#include
-
-namespace base {
-
- struct TwistWithCovariance
- {
- public:
- typedef base::Matrix6d Covariance;
-
- public:
-
- /** Velocity in m/s **/
- base::Vector3d vel;
-
- /** Rotation rate in rad/s **/
- base::Vector3d rot;
-
- /** Uncertainty **/
- Covariance cov;
-
- public:
- explicit TwistWithCovariance (const base::Vector3d& vel = base::Vector3d::Zero(), const base::Vector3d& rot = base::Vector3d::Zero() ):
- vel(vel), rot(rot){this->invalidateCovariance(); };
-
- TwistWithCovariance(const base::Vector3d& vel, const base::Vector3d& rot, const Covariance& cov):
- vel(vel), rot(rot), cov(cov) {};
-
- TwistWithCovariance(const base::Vector6d& velocity, const Covariance& cov)
- {
- this->setVelocity(velocity);
- this->cov = cov;
- }
-
- /** Default std::cout function **/
- friend std::ostream & operator<<(std::ostream &out, const base::Vector6d& vel);
-
- /** Get and Set Methods **/
- const base::Vector3d& getTranslation() const { return this->vel; }
- void setTranslation(const base::Vector3d& vel) { this->vel = vel; }
-
- const base::Vector3d& getRotation() const { return this->rot; }
- void setRotation(const base::Vector3d& rot) { this->rot = rot; }
-
- const Covariance& getCovariance() const { return this->cov; }
- void setCovariance(const Covariance& cov) { this->cov = cov; }
-
- const base::Matrix3d getLinearVelocityCov() const { return this->cov.block<3,3>(0,0); }
- void setLinearVelocityCov(const base::Matrix3d& cov) { this->cov.block<3,3>(0,0) = cov; }
- const base::Matrix3d getAngularVelocityCov() const { return this->cov.block<3,3>(3,3); }
- void setAngularVelocityCov(const base::Matrix3d& cov) { this->cov.block<3,3>(3,3) = cov; }
-
- const base::Vector3d& getLinearVelocity() const {return this->getTranslation(); }
- void setLinearVelocity(const base::Vector3d& vel) { return this->setTranslation(vel); }
- const base::Vector3d& getAngularVelocity() const {return this->getRotation(); }
- void setAngularVelocity(const base::Vector3d& rot) { return this->setRotation(rot); }
-
- const base::Vector3d& translation() const {return this->getTranslation(); }
- const base::Vector3d& rotation() const {return this->getRotation(); }
-
- const base::Vector6d getVelocity() const
- {
- base::Vector6d all_velocities;
- all_velocities.block<3,1>(0,0) = this->vel;
- all_velocities.block<3,1>(3,0) = this->rot;
- return all_velocities;
- }
-
- void setVelocity(const base::Vector6d& velocity)
- {
- /** Linear velocity at first place, Angular velocity at second place **/
- this->vel = velocity.block<3,1>(0,0);
- this->rot = velocity.block<3,1>(3,0);
- }
-
- /** Check Methods **/
- bool hasValidVelocity() const
- {
- return base::isnotnan(this->vel) && base::isnotnan(this->rot);
- }
-
- void invalidateVelocity()
- {
- this->vel = base::Vector3d::Ones() * base::unknown();
- this->rot = base::Vector3d::Ones() * base::unknown();
- }
-
- bool hasValidCovariance() const { return base::isnotnan(this->cov); }
- void invalidateCovariance()
- {
- this->cov = Covariance::Ones() * base::unknown();
- }
-
- void invalidate()
- {
- this->invalidateVelocity();
- this->invalidateCovariance();
- }
-
- static TwistWithCovariance Zero()
- {
- return TwistWithCovariance(static_cast(base::Vector3d::Zero()), static_cast(base::Vector3d::Zero()));
- }
-
- double& operator[](int i)
- {
- if (i<3)
- return this->vel(i);
- else
- return this->rot(i-3);
- }
-
- double operator[](int i) const
- {
- if (i<3)
- return this->vel(i);
- else
- return this->rot(i-3);
- }
-
-
- inline TwistWithCovariance& operator+=(const TwistWithCovariance& arg)
- {
- this->vel += arg.vel;
- this->rot += arg.rot;
- if (this->hasValidCovariance() && arg.hasValidCovariance())
- {
- this->cov += arg.cov;
- base::guaranteeSPD(this->cov);
- }
- return *this;
- }
-
- inline friend TwistWithCovariance operator+(TwistWithCovariance lhs,const TwistWithCovariance& rhs)
- {
- return lhs += rhs;
- }
-
- inline TwistWithCovariance& operator-=(const TwistWithCovariance& arg)
- {
- this->vel -= arg.vel;
- this->rot -= arg.rot;
- if (this->hasValidCovariance() && arg.hasValidCovariance())
- {
- this->cov += arg.cov;
- base::guaranteeSPD(this->cov);
- }
-
- return *this;
- }
-
- inline friend TwistWithCovariance operator-(TwistWithCovariance lhs, const TwistWithCovariance& rhs)
- {
- return lhs -= rhs;
- }
-
- inline friend TwistWithCovariance operator*(const TwistWithCovariance& lhs,double rhs)
- {
- if (!lhs.hasValidCovariance())
- {
- return TwistWithCovariance(static_cast(lhs.vel*rhs), static_cast(lhs.rot*rhs));
- }
- else
- {
- return TwistWithCovariance(static_cast(lhs.vel*rhs), static_cast(lhs.rot*rhs), static_cast((rhs*rhs)*lhs.cov));
- }
- }
-
- inline friend TwistWithCovariance operator*(double lhs, const TwistWithCovariance& rhs)
- {
- if (!rhs.hasValidCovariance())
- {
- return TwistWithCovariance(static_cast(lhs*rhs.vel), static_cast(lhs*rhs.rot));
- }
- else
- {
- return TwistWithCovariance(static_cast(lhs*rhs.vel), static_cast(lhs*rhs.rot), static_cast((lhs*lhs)*rhs.cov));
- }
- }
-
-
- //Spatial Cross product
- inline friend TwistWithCovariance operator*(const TwistWithCovariance& lhs, const TwistWithCovariance& rhs)
- {
- TwistWithCovariance tmp;
- tmp.vel = lhs.rot.cross(rhs.vel)+lhs.vel.cross(rhs.rot);
- tmp.rot = lhs.rot.cross(rhs.rot);
-
- /** In case the two twist have uncertainty **/
- if (lhs.hasValidCovariance() && rhs.hasValidCovariance())
- {
- Eigen::Matrix cross_jacob;
- Eigen::Matrix cross_cov;
-
- /** Initialize covariance **/
- tmp.cov.setZero();
-
- cross_jacob = TwistWithCovariance::crossJacobian(lhs.rot, rhs.vel);
- cross_cov << lhs.cov.block<3,3>(3,3), base::Matrix3d::Zero(),
- base::Matrix3d::Zero(), rhs.cov.block<3,3>(0,0);
-
- /** Linear velocity is at the first covariance block **/
- tmp.cov.block<3,3>(0,0) = cross_jacob * cross_cov * cross_jacob.transpose();
-
- cross_jacob = TwistWithCovariance::crossJacobian(lhs.vel, rhs.rot);
- cross_cov << lhs.cov.block<3,3>(0,0), base::Matrix3d::Zero(),
- base::Matrix3d::Zero(),rhs.cov.block<3,3>(3,3);
-
- /** Linear velocity is at the first covariance block **/
- tmp.cov.block<3,3>(0,0) += cross_jacob * cross_cov * cross_jacob.transpose();
-
- cross_jacob = TwistWithCovariance::crossJacobian(lhs.rot, rhs.rot);
- cross_cov << lhs.cov.block<3,3>(3,3), base::Matrix3d::Zero(),
- base::Matrix3d::Zero(),rhs.cov.block<3,3>(3,3);
-
- /** Angular velocity is at the first covariance block **/
- tmp.cov.block<3,3>(3,3) = cross_jacob * cross_cov * cross_jacob.transpose();
-
- base::guaranteeSPD(tmp.cov);
- }
-
- return tmp;
- }
-
- inline friend TwistWithCovariance operator/(const TwistWithCovariance& lhs,double rhs)
- {
- return TwistWithCovariance(static_cast(lhs.vel/rhs), static_cast(lhs.rot/rhs), static_cast((1.0/(rhs *rhs))*lhs.cov));
- }
-
-
- /** unary - **/
- inline friend TwistWithCovariance operator-(const TwistWithCovariance& arg)
- {
- return TwistWithCovariance(static_cast(-arg.vel), static_cast(-arg.rot), arg.cov);
- }
-
- static Eigen::Matrix crossJacobian(const base::Vector3d& u, const base::Vector3d& v)
- {
- Eigen::Matrix cross_jacob;
- base::Matrix3d cross_u, cross_v;
- cross_u << 0.0, -u[2], u[1], u[2], 0.0, -u[0], -u[1], u[0], 0.0;
- cross_v << 0.0, -v[2], v[1], v[2], 0.0, -v[0], -v[1], v[0], 0.0;
- cross_jacob << cross_u, cross_v;
- return cross_jacob;
- }
-
- };
-
- /** Default std::cout function
- */
- inline std::ostream & operator<<(std::ostream &out, const base::TwistWithCovariance& twist)
- {
- /** cout the 6D twist vector (rotational first and linear second) with its associated covariance matrix **/
- for (register unsigned short i=0; i
-#include
-#include
-#include
-#include
-#include /** For backward compatibility with RBS **/
-
-#include
-#include
-
-namespace base { namespace samples {
-
- /** Representation of the state of a rigid body
- *
- * This is among other things used to express frame transformations by
- * Rock's transformer
- *
- * Given a source and target frame, this structure expresses the _frame
- * change_ between these two frames. In effect, it represents the state of
- * the source frame expressed in the target frame.
- *
- * Per [Rock's conventions](http://rock.opendfki.de/wiki/WikiStart/Standards), you
- * should use a X-forward, right handed coordinate system when assigning
- * frames to bodies (i.e. X=forward, Y=left, Z=up). In addition,
- * world-fixed frames should be aligned to North (North-East-Up)
- *
- * For instance, if sourceFrame is "body" and targetFrame is "world", then
- * the BodyState object is the state of body in the world frame
- * (usually, the world frame has an arbitrary origin and a North-East-Up
- * orientation).
- */
- struct BodyState
- {
-
- BodyState(bool doInvalidation=true)
- {
- if(doInvalidation)
- invalidate();
- };
-
- BodyState(const base::TransformWithCovariance& pose, const base::TwistWithCovariance& velocity):
- pose(pose), velocity(velocity) {};
-
- /** Time-stamp **/
- base::Time time;
-
- /** Robot pose: rotation in radians and translation in meters */
- base::TransformWithCovariance pose;
-
- /** TwistWithCovariance: Linear[m/s] and Angular[rad/s] Velocity of the Body */
- /** It is assumed here that velocity is the derivative of a delta pose for
- * a given interval. When such interval tends to zero, one could talk
- * of instantaneous velocity **/
- base::TwistWithCovariance velocity;
-
- void setPose(const base::Affine3d& pose)
- {
- this->pose.setTransform(pose);
- }
-
- const base::Affine3d getPose() const
- {
- return this->pose.getTransform();
- }
-
- double getYaw() const
- {
- return base::getYaw(this->pose.orientation);
- }
-
- double getPitch() const
- {
- return base::getPitch(this->pose.orientation);
- }
-
- double getRoll() const
- {
- return base::getRoll(this->pose.orientation);
- }
-
- inline const base::Position& position() const
- {
- return this->pose.translation;
- }
-
- inline base::Position& position()
- {
- return this->pose.translation;
- }
-
- /** A read-only expression of the rotation **/
- inline const base::Quaterniond& orientation() const
- {
- return this->pose.orientation;
- }
-
- inline base::Quaterniond& orientation()
- {
- return this->pose.orientation;
- }
-
- inline const base::Vector3d& linear_velocity() const
- {
- return this->velocity.vel;
- }
-
- inline base::Position& linear_velocity()
- {
- return this->velocity.vel;
- }
-
- inline const base::Vector3d& angular_velocity() const
- {
- return this->velocity.rot;
- }
-
- inline base::Position& angular_velocity()
- {
- return this->velocity.rot;
- }
-
-
- /** A read-only expression of the pose covariance **/
- inline const base::Matrix6d& cov_pose() const
- {
- return this->pose.cov;
- }
-
- inline base::Matrix6d& cov_pose()
- {
- return this->pose.cov;
- }
-
- /** A read-only expression of the rotation covariance **/
- inline const base::Matrix3d cov_orientation() const
- {
- return this->pose.getOrientationCov();
- }
-
- inline void cov_orientation(const base::Matrix3d& cov)
- {
- return this->pose.setOrientationCov(cov);
- }
-
- /** A read-only expression of the position covariance **/
- inline const base::Matrix3d cov_position() const
- {
- return this->pose.getTranslationCov();
- }
-
- inline void cov_position(const base::Matrix3d& cov)
- {
- return this->pose.setTranslationCov(cov);
- }
-
- /** A read-only expression of the velocity covariance **/
- inline const base::Matrix6d& cov_velocity() const
- {
- return this->velocity.cov;
- }
-
- inline base::Matrix6d& cov_velocity()
- {
- return this->velocity.cov;
- }
-
- /** A read-only expression of the linear velocity covariance **/
- inline const base::Matrix3d cov_linear_velocity() const
- {
- return this->velocity.getLinearVelocityCov();
- }
-
- inline void cov_linear_velocity(const base::Matrix3d& cov)
- {
- return this->velocity.setLinearVelocityCov(cov);
- }
-
- /** A read-only expression of the angular velocity covariance **/
- inline const base::Matrix3d cov_angular_velocity() const
- {
- return this->velocity.getAngularVelocityCov();
- }
-
- inline void cov_angular_velocity(const base::Matrix3d& cov)
- {
- return this->velocity.setAngularVelocityCov(cov);
- }
-
- static BodyState Unknown()
- {
- BodyState result(false);
- result.initUnknown();
- return result;
- };
-
- static BodyState Invalid()
- {
- BodyState result(true);
- return result;
- };
-
- /** For backward compatibility only. Use invalidate() */
- void initSane()
- {
- invalidate();
- }
-
- /** Initializes the rigid body state with NaN for the
- * position, velocity, orientation and angular velocity.
- */
- void invalidate()
- {
- invalidatePose();
- invalidatePoseCovariance();
- invalidateVelocity();
- invalidateVelocityCovariance();
- }
-
- /**
- * Initializes the rigid body state unknown with Zero for the
- * position, velocity and angular velocity, Identity for the orientation
- * and infinity for all covariances.
- */
- void initUnknown()
- {
- this->pose.setTransform(base::Affine3d::Identity());
- this->pose.invalidateCovariance();
- this->velocity.setVelocity(base::Vector6d::Zero());
- this->velocity.invalidateCovariance();
- }
-
- bool hasValidPose() const { return this->pose.hasValidTransform(); }
- bool hasValidPoseCovariance() const { return this->pose.hasValidCovariance(); }
- void invalidatePose() { this->pose.invalidateTransform(); }
- void invalidatePoseCovariance() { this->pose.invalidateCovariance(); }
-
- bool hasValidVelocity() const { return this->velocity.hasValidVelocity(); }
- bool hasValidVelocityCovariance() const { return this->velocity.hasValidCovariance(); }
- void invalidateVelocity() { this->velocity.invalidateVelocity(); }
- void invalidateVelocityCovariance() { this->velocity.invalidateCovariance(); }
-
- void invalidateValues ( bool pose = true, bool velocity = true)
- {
- if (pose) this->invalidatePose();
- if (velocity) this->invalidateVelocity();
- }
-
- void invalidateCovariances ( bool pose = true, bool velocity = true)
- {
- if (pose) this->invalidatePoseCovariance();
- if (velocity) this->invalidateVelocityCovariance();
- }
-
- /** For backward compatibility with RBS **/
- BodyState& operator=( const base::samples::RigidBodyState& rbs )
- {
- /** extract the transformation **/
- this->pose.setTransform(rbs.getTransform());
-
- /** and the transformation covariance **/
- this->pose.cov << rbs.cov_orientation, Eigen::Matrix3d::Zero(),
- Eigen::Matrix3d::Zero(), rbs.cov_position;
-
- /** Extract the velocity **/
- this->velocity.vel = rbs.velocity;
- this->velocity.rot = rbs.angular_velocity;
-
- /** Extract the velocity covariance **/
- this->velocity.cov << rbs.cov_angular_velocity, Eigen::Matrix3d::Zero(),
- Eigen::Matrix3d::Zero(), rbs.cov_velocity;
-
- return *this;
- };
-
-
- /** Default std::cout function
- */
- friend std::ostream & operator<<(std::ostream &out, const TransformWithCovariance& trans);
-
- /** performs a composition of this Body State with the Body State given.
- * The result is another Body State with result = this * trans
- */
- BodyState composition( const BodyState& bs ) const
- {
- return this->operator*( bs );
- };
-
- /** alias for the composition of two body states
- */
- BodyState operator*( const BodyState& bs ) const
- {
- const BodyState &bs2(*this);
- const BodyState &bs1(bs);
-
- /** The composition of two body states is here defined as the
- * composition of the pose transformation as it is defined in the
- * TransformWithCovariance. The velocity held by the last body
- * state (here bs1) is assumed to be the current "instantaneous"
- * body state velocity and of the resulting body state.
- **/
- base::TransformWithCovariance result_pose (static_cast(bs2.pose * bs1.pose));
- base::TwistWithCovariance result_velocity (bs1.velocity);
-
- /** Resulting velocity and covariance with respect to the pose base frame **/
- result_velocity.vel = result_pose.orientation * result_velocity.vel;
- result_velocity.rot = result_pose.orientation * result_velocity.rot;
- if (result_velocity.hasValidCovariance())
- {
- /** Uncertainty propagation through assuming linear transformation. From R. Astudillo and R.Kolossa Chapter 3 Uncertainty Propagation **/
- /** Improvement at this point(TO-DO): change to Jacobian derivation since the propagation is not linear **/
- Eigen::Matrix3d rot_matrix(result_pose.orientation.toRotationMatrix());
- result_velocity.cov.block<3,3>(0,0) = (rot_matrix * result_velocity.cov.block<3,3>(0,0) * rot_matrix.transpose());
- result_velocity.cov.block<3,3>(3,3) = (rot_matrix * result_velocity.cov.block<3,3>(3,3) * rot_matrix.transpose());
- result_velocity.cov.block<3,3>(3,0) = (rot_matrix * result_velocity.cov.block<3,3>(3,0) * rot_matrix.transpose());
- result_velocity.cov.block<3,3>(0,3) = (rot_matrix * result_velocity.cov.block<3,3>(0,3) * rot_matrix.transpose());
- }
-
- /* Result Body State **/
- return BodyState(result_pose, result_velocity);
- }
- };
-
- /** Default std::cout function
- */
- inline std::ostream & operator<<(std::ostream &out, const base::samples::BodyState& bs)
- {
- out << bs.pose << "\n";
- out << bs.velocity << "\n";
- return out;
- };
-
-}}//end namespace base::samples
-#endif
-
diff --git a/base/samples/CompressedFrame.hpp b/base/samples/CompressedFrame.hpp
deleted file mode 100644
index 4ff830c5..00000000
--- a/base/samples/CompressedFrame.hpp
+++ /dev/null
@@ -1,47 +0,0 @@
-#ifndef _COMPRESSED_FRAME_H_
-#define _COMPRESSED_FRAME_H_
-
-#include
-
-namespace base { namespace samples { namespace frame {
- enum frame_compressed_mode_t {
- MODE_COMPRESSED_UNDEFINED = 0,
- MODE_PJPG = 1
- };
-
- struct CompressedFrame{
-
- /** The time at which this frame has been captured
- *
- * This is obviously an estimate
- */
- base::Time time;
- /** The time at which this frame has been received on the system */
- base::Time received_time;
-
- /** The raw data */
- std::vector image;
- /** Additional metadata */
- std::vector attributes;
-
- /** The image size in pixels */
- frame_size_t size;
-
- frame_compressed_mode_t frame_mode;
-
- /** Status flag */
- frame_status_t frame_status;
-
- static frame_compressed_mode_t toFrameMode(const std::string &str)
- {
- if(str == "MODE_COMPRESSED_UNDEFINED")
- return MODE_COMPRESSED_UNDEFINED;
- else if (str == "MODE_PJPG")
- return MODE_PJPG;
- }
- };
-
-
-}}};
-
-#endif
diff --git a/base/samples/Frame.hpp b/base/samples/Frame.hpp
deleted file mode 100644
index fb9fae60..00000000
--- a/base/samples/Frame.hpp
+++ /dev/null
@@ -1,562 +0,0 @@
-/*! \file frame.h
- \brief container for imaging data
-*/
-
-#ifndef BASE_SAMPLES_FRAME_H__
-#define BASE_SAMPLES_FRAME_H__
-
-#include
-#include
-
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include
-
-
-namespace base { namespace samples { namespace frame {
- struct frame_attrib_t
- {
- std::string data_;
- std::string name_;
- inline void set(const std::string &name,const std::string &data)
- {
- name_ = name;
- data_ = data;
- }
- };
-
- struct frame_size_t {
- frame_size_t() : width(0), height(0) {}
- frame_size_t(uint16_t w, uint16_t h) : width(w), height(h) {}
-
- bool operator==(const frame_size_t &other) const
- {
- if(width == other.width && height==other.height)
- return true;
- return false;
- };
-
- bool operator!=(const frame_size_t &other) const
- {
- return !(*this == other);
- };
- uint16_t width;
- uint16_t height;
- };
-
- enum frame_mode_t {
- MODE_UNDEFINED = 0,
- MODE_GRAYSCALE = 1,
- MODE_RGB = 2,
- MODE_UYVY = 3,
- MODE_BGR = 4,
- MODE_RGB32 = 5,
- RAW_MODES = 128,
- MODE_BAYER = RAW_MODES + 0,
- MODE_BAYER_RGGB = RAW_MODES + 1,
- MODE_BAYER_GRBG = RAW_MODES + 2,
- MODE_BAYER_BGGR = RAW_MODES + 3,
- MODE_BAYER_GBRG = RAW_MODES + 4,
- COMPRESSED_MODES = 256, //if an image is compressed it has no relationship
- //between number of pixels and number of bytes
- MODE_PJPG = COMPRESSED_MODES + 1,
- MODE_JPEG = COMPRESSED_MODES + 2,
- MODE_PNG = COMPRESSED_MODES + 3
- };
-
- enum frame_status_t {
- STATUS_EMPTY,
- STATUS_VALID,
- STATUS_INVALID
- };
-
- /* A single image frame */
- struct Frame
- {
- public:
- /**
- * Initialize the frame
- * @param width the image width, in pixels
- * @param height the image height, in pixels
- * @param data_depth the number of effective bits per pixels
- * @param mode the frame mode (raw, hdr, greyscale, colour)
- */
- Frame() :
- image(), size(), data_depth(0), pixel_size(0),
- frame_mode()
- {
- setDataDepth(0);
- reset();
- }
-
- //@depth number of bits per pixel and channel
- Frame(uint16_t width, uint16_t height, uint8_t depth=8, frame_mode_t mode=MODE_GRAYSCALE, uint8_t const val = 0,size_t sizeInBytes=0)
- {
- init(width,height,depth,mode,val,sizeInBytes);
- }
-
- //makes a copy of other
- Frame(const Frame &other,bool bcopy = true)
- {
- init(other,bcopy);
- }
-
- //copies all attributes which are independent from size and mode
- //if an attribute already exists the old value is over written
- void copyImageIndependantAttributes(const Frame &other)
- {
- std::vector::const_iterator iter = other.attributes.begin();
- for(;iter!= other.attributes.end();++iter)
- setAttribute(iter->name_,iter->data_);
- time = other.time;
- received_time = other.received_time;
- frame_status = other.frame_status;
- }
-
- //makes a copy of other
- void init(const Frame &other,bool bcopy = true)
- {
- //hdr is copied by attributes = other.attributes;
- init(other.getWidth(),other.getHeight(),other.getDataDepth(), other.getFrameMode(),-1,other.getNumberOfBytes());
- if(bcopy)
- setImage(other.getImage());
- copyImageIndependantAttributes(other);
- }
-
- void init(uint16_t width, uint16_t height, uint8_t depth=8, frame_mode_t mode=MODE_GRAYSCALE, const uint8_t val = 0, size_t sizeInBytes=0)
- {
- //change size if the frame does not fit
- if(this->size.height != height || this->size.width != width || this->frame_mode != mode ||
- this->data_depth != depth || (sizeInBytes != 0 && sizeInBytes != image.size()))
- {
- //check if depth = 0
- //this might be a programmer error
- if(depth==0 && (height != 0 || width != 0 ))
- throw std::runtime_error("Frame::init: Cannot initialize frame with depth = 0.");
-
- this->frame_mode = mode;
- this->size = frame_size_t(width, height);
- setDataDepth(depth);
- }
- //calculate size if not given
- if(!sizeInBytes)
- sizeInBytes = getPixelSize() * getPixelCount();
-
- validateImageSize(sizeInBytes);
- image.resize(sizeInBytes);
- reset(val);
- }
-
- // if val is negative the image will not be initialized
- void reset(int const val = 0)
- {
- this->time = base::Time();
- if (this->image.size() > 0 && val >= 0) {
- memset(&this->image[0], val%256, this->image.size());
- }
- setStatus(STATUS_EMPTY);
- attributes.clear();
- }
-
- void swap(Frame &frame)
- {
- //swap vector
- image.swap(frame.image);
- attributes.swap(frame.attributes);
-
- //copy values to temp
- base::Time temp_time = frame.time;
- base::Time temp_received_time = frame.received_time;
- frame_size_t temp_size = frame.size;
- uint32_t temp_data_depth = frame.data_depth;
- uint32_t temp_pixel_size = frame.pixel_size;
- uint32_t temp_row_size = frame.row_size;
- frame_mode_t temp_frame_mode = frame.frame_mode;
- frame_status_t temp_frame_status = frame.frame_status;
-
- //copy values
- frame.time = time;
- frame.received_time = received_time;
- frame.size = size;
- frame.data_depth = data_depth;
- frame.pixel_size = pixel_size;
- frame.row_size = row_size;
- frame.frame_mode = frame_mode;
- frame.frame_status = frame_status;
-
- time = temp_time;
- received_time = temp_received_time;
- size = temp_size;
- data_depth = temp_data_depth;
- pixel_size = temp_pixel_size;
- row_size = temp_row_size;
- frame_mode = temp_frame_mode;
- frame_status = temp_frame_status;
- }
-
- inline bool isHDR()const
- {
- return (hasAttribute("hdr")&&getAttribute("hdr"));
- }
-
- inline void setHDR(bool value)
- {
- setAttribute("hdr",true);
- }
-
- inline bool isCompressed()const
- {
- return frame_mode >= COMPRESSED_MODES;
- }
-
- inline bool isGrayscale()const {
- return this->frame_mode == MODE_GRAYSCALE;
- }
- inline bool isRGB()const {
- return this->frame_mode == MODE_RGB;
- }
-
- inline bool isBayer()const {
- return (this->frame_mode == MODE_BAYER || this->frame_mode == MODE_BAYER_RGGB || this->frame_mode == MODE_BAYER_GRBG || this->frame_mode == MODE_BAYER_BGGR || this->frame_mode == MODE_BAYER_GBRG);
- }
-
- inline void setStatus(const frame_status_t value){
- frame_status = value;
- }
-
- inline frame_status_t getStatus()const{
- return frame_status;
- }
-
- inline uint32_t getChannelCount() const {
- return getChannelCount(this->frame_mode);
- }
- static uint32_t getChannelCount(frame_mode_t mode)
- {
- switch (mode)
- {
- case MODE_UNDEFINED:
- return 0;
- case MODE_BAYER:
- case MODE_BAYER_RGGB:
- case MODE_BAYER_BGGR:
- case MODE_BAYER_GBRG:
- case MODE_BAYER_GRBG:
- case MODE_GRAYSCALE:
- case MODE_UYVY:
- return 1;
- case MODE_RGB:
- case MODE_BGR:
- return 3;
- case MODE_RGB32:
- return 4;
- case MODE_PJPG:
- case MODE_JPEG:
- case MODE_PNG:
- return 1;
- default:
- throw std::runtime_error("Frame::getChannelCount: Unknown frame_mode");
- return 0;
- }
- }
-
- //qt ruby does not support enums as slot parameters
- //therefore frame_mode_t is passed as string
- static frame_mode_t toFrameMode(const std::string &str)
- {
- if(str == "MODE_UNDEFINED")
- return MODE_UNDEFINED;
- else if (str == "MODE_GRAYSCALE")
- return MODE_GRAYSCALE;
- else if (str == "MODE_RGB")
- return MODE_RGB;
- else if (str == "MODE_BGR")
- return MODE_BGR;
- else if (str == "MODE_UYVY")
- return MODE_UYVY;
- else if (str == "RAW_MODES")
- return RAW_MODES;
- else if (str == "MODE_BAYER")
- return MODE_BAYER;
- else if (str == "MODE_BAYER_RGGB")
- return MODE_BAYER_RGGB;
- else if (str == "MODE_BAYER_GRBG")
- return MODE_BAYER_GRBG;
- else if (str == "MODE_BAYER_BGGR")
- return MODE_BAYER_BGGR;
- else if (str == "MODE_BAYER_GBRG")
- return MODE_BAYER_GBRG;
- else if (str == "MODE_RGB32")
- return MODE_RGB32;
- else if (str == "COMPRESSED_MODES")
- return COMPRESSED_MODES;
- else if (str == "MODE_PJPG")
- return MODE_PJPG;
- else if (str == "MODE_JPEG")
- return MODE_JPEG;
- else if (str == "MODE_PNG")
- return MODE_PNG;
- else
- return MODE_UNDEFINED;
- };
-
- inline frame_mode_t getFrameMode() const {
- return this->frame_mode;
- }
-
- /**
- * Returns the size of a pixel (in bytes). This takes into account the image
- * mode as well as the data depth.
- * @return Number of channels * bytes used to represent one colour
- */
- inline uint32_t getPixelSize() const {
- return this->pixel_size;
- }
-
- /**
- * Returns the size of a row (in bytes). This takes into account the image
- * mode as well as the data depth.
- * @return Number of channels * width * bytes used to represent one colour
- * @return 0 if the image is compressed
- */
- inline uint32_t getRowSize() const {
- if(isCompressed())
- throw std::runtime_error("Frame::getRowSize: There is no raw size for an compressed image!");
- return this->row_size;
- }
-
- /**
- * Returns the total number of bytes for the image
- */
- inline uint32_t getNumberOfBytes() const {
- return image.size();
- }
-
- /**
- * Returns the total count of pixels in this frame
- * @return Returns the overall number of pixels (width * height)
- */
- inline uint32_t getPixelCount() const {
- return size.width * size.height;
- }
-
- inline uint32_t getDataDepth() const {
- return this->data_depth;
- }
- void setDataDepth(uint32_t value)
- {
- this->data_depth = value;
-
- // Update pixel size
- uint32_t comp_size = ((this->data_depth + 7) / 8);
- this->pixel_size = getChannelCount(this->frame_mode) * comp_size;
-
- //update row size
- if(isCompressed())
- this->row_size = 0; //disable row size
- else
- this->row_size = this->pixel_size * getWidth();
-
- }
-
- void setFrameMode(frame_mode_t mode)
- {
- this->frame_mode = mode;
-
- // Update pixel size
- uint32_t comp_size = ((this->data_depth + 7) / 8);
- this->pixel_size = getChannelCount(this->frame_mode) * comp_size;
-
- //update row size
- if(isCompressed())
- this->row_size = 0; //disable row size
- else
- this->row_size = this->pixel_size * getWidth();
- }
-
- inline frame_size_t getSize() const {
- return this->size;
- }
- inline uint16_t getWidth() const {
- return this->size.width;
- }
- inline uint16_t getHeight() const {
- return this->size.height;
- }
-
- inline const std::vector &getImage() const {
- return this->image;
- }
-
- void validateImageSize(size_t sizeToValidate) const {
- size_t expected_size = getPixelSize()*getPixelCount();
- if (!isCompressed() && sizeToValidate != expected_size){
- std::cerr << "Frame: "
- << __FUNCTION__ << " (" << __FILE__ << ", line "
- << __LINE__ << "): " << "image size mismatch in setImage() ("
- << "getting " << sizeToValidate << " bytes but I was expecting " << expected_size << " bytes)"
- << std::endl;
- throw std::runtime_error("Frame::validateImageSize: wrong image size!");
- }
- }
- inline void setImage(const std::vector &newImage) {
- // calling the overloading function wich uses the "char*" interface
- return setImage(newImage.data(), newImage.size());
- }
- /** This is for backward compatibility for the people that were
- * using the 'char' signature */
- inline void setImage(const char *data, size_t newImageSize) {
- return setImage(reinterpret_cast(data), newImageSize);
- }
- inline void setImage(const uint8_t *data, size_t newImageSize) {
- validateImageSize(newImageSize);
- image.resize(newImageSize);
- memcpy(&this->image[0], data, newImageSize);
- }
-
- inline uint8_t *getImagePtr() {
- return static_cast(image.data());
- }
- inline const uint8_t *getImageConstPtr() const {
- return static_cast(image.data());
- }
-
- inline uint8_t* getLastByte(){
- return static_cast(&this->image.back());
- }
-
- inline const uint8_t* getLastConstByte()const{
- return static_cast(&this->image.back());
- }
-
- inline bool hasAttribute(const std::string &name)const
- {
- std::vector::const_iterator _iter = attributes.begin();
- for (;_iter != attributes.end();_iter++)
- {
- if (_iter->name_ == name)
- return true;
- }
- return false;
- }
- template
- inline T getAttribute(const std::string &name)const
- {
- static T default_value;
- std::stringstream strstr;
-
- std::vector::const_iterator _iter = attributes.begin();
- for (;_iter != attributes.end();_iter++)
- {
- if (_iter->name_ == name)
- {
- T data;
- strstr << _iter->data_;
- strstr >> data;
- return data;
- }
- }
- return default_value;
- }
-
- inline bool deleteAttribute(const std::string &name)
- {
- std::vector::iterator _iter = attributes.begin();
- for (;_iter != attributes.end();_iter++)
- {
- if (_iter->name_ == name)
- {
- attributes.erase(_iter);
- return true;
- }
- }
- return false;
- }
-
-
- template
- inline void setAttribute(const std::string &name,const T &data)
- {
- //if attribute exists
- std::stringstream strstr;
- strstr << data;
- std::vector::iterator _iter = attributes.begin();
- for (;_iter != attributes.end();_iter++)
- {
- if (_iter->name_ == name)
- {
- _iter->set(name,strstr.str());
- return;
- }
- }
- //if attribute does not exist
- attributes.push_back(frame_attrib_t());
- attributes.back().set(name,strstr.str());
- return ;
- }
-
- template Tp& at(unsigned int column,unsigned int row)
- {
- if(column >= size.width || row >= size.height )
- throw std::runtime_error("out of index");
- return *((Tp*)(getImagePtr()+row*getRowSize()+column*getPixelSize()));
- }
-
- /** The time at which this frame has been captured
- *
- * This is obviously an estimate
- */
- base::Time time;
- /** The time at which this frame has been received on the system */
- base::Time received_time;
-
- /** The raw data */
- std::vector image;
- /** Additional metadata */
- std::vector attributes;
-
- /** The image size in pixels */
- frame_size_t size;
-
- /** The number of effective bits per channel. The number
- * of actual bits per channel is always a multiple of
- * height (i.e. a 12-bit effective depth is represented
- * using 16-bits per channels). The number of greyscale
- * levels is 2^(this_number)
- */
- uint32_t data_depth;
- /** The size of one pixel, in bytes
- *
- * For instance, for a RGB image with a 8 bit data depth, it would
- * be 3. For a 12 bit non-packed image (i.e with each channel
- * encoded on 2 bytes), it would be 6.
- */
- uint32_t pixel_size;
-
- /** The size of a complete row in bytes
- */
- uint32_t row_size;
-
- /** The colorspace of the image
- */
- frame_mode_t frame_mode;
-
- /** Status flag */
- frame_status_t frame_status;
- };
-
- struct FramePair
- {
- base::Time time;
- Frame first;
- Frame second;
- uint32_t id;
- };
-}}}
-
-#endif
diff --git a/base/samples/Joints.hpp b/base/samples/Joints.hpp
deleted file mode 100644
index 5a13109a..00000000
--- a/base/samples/Joints.hpp
+++ /dev/null
@@ -1,116 +0,0 @@
-#ifndef BASE_SAMPLES_JOINTS_HPP
-#define BASE_SAMPLES_JOINTS_HPP
-
-#include
-#include
-
-#include
-#include
-#include
-
-namespace base
-{
- namespace samples
- {
- /** Data structure that gives out state readings for a set of joints
- */
- struct Joints : public base::NamedVector
- {
- /** The sample timestamp */
- base::Time time;
-
- static Joints Positions(std::vector const& positions)
- {
- Joints result;
- result.elements.resize(positions.size());
- for (std::size_t i = 0; i != positions.size(); ++i)
- result[i].position = positions[i];
- return result;
- }
-
- static Joints Positions(std::vector const& positions, std::vector const& names)
- {
- Joints result = Positions(positions);
- if (result.elements.size() != names.size())
- throw std::runtime_error("the position and names vectors differ");
- result.names = names;
- return result;
- }
-
- static Joints Speeds(std::vector const& speeds)
- {
- Joints result;
- result.elements.resize(speeds.size());
- for (std::size_t i = 0; i != speeds.size(); ++i)
- result[i].speed= speeds[i];
- return result;
- }
-
- static Joints Speeds(std::vector const& speeds, std::vector const& names)
- {
- Joints result = Speeds(speeds);
- if (result.elements.size() != names.size())
- throw std::runtime_error("the speeds and names vectors differ");
- result.names = names;
- return result;
- }
-
- static Joints Efforts(std::vector const& efforts)
- {
- Joints result;
- result.elements.resize(efforts.size());
- for (std::size_t i = 0; i != efforts.size(); ++i)
- result[i].effort = efforts[i];
- return result;
- }
-
- static Joints Efforts(std::vector const& efforts, std::vector const& names)
- {
- Joints result = Efforts(efforts);
- if (result.elements.size() != names.size())
- throw std::runtime_error("the effort and names vectors differ");
- result.names = names;
- return result;
- }
-
- static Joints Raw(std::vector const& raw)
- {
- Joints result;
- result.elements.resize(raw.size());
- for (std::size_t i = 0; i != raw.size(); ++i)
- result[i].raw = raw[i];
- return result;
- }
-
- static Joints Raw(std::vector const& raw, std::vector const& names)
- {
- Joints result = Raw(raw);
- if (result.elements.size() != names.size())
- throw std::runtime_error("the raw and names vectors differ");
- result.names = names;
- return result;
- }
-
- static Joints Accelerations(std::vector const& acceleration)
- {
- Joints result;
- result.elements.resize(acceleration.size());
- for (std::size_t i = 0; i != acceleration.size(); ++i)
- result[i].acceleration = acceleration[i];
- return result;
- }
-
- static Joints Accelerations(std::vector const& acceleration, std::vector const& names)
- {
- Joints result = Accelerations(acceleration);
- if (result.elements.size() != names.size())
- throw std::runtime_error("the acceleration and names vectors differ");
- result.names = names;
- return result;
- }
- };
- }
-}
-
-#endif
-
diff --git a/base/samples/RigidBodyState.hpp b/base/samples/RigidBodyState.hpp
deleted file mode 100644
index 06de3209..00000000
--- a/base/samples/RigidBodyState.hpp
+++ /dev/null
@@ -1,293 +0,0 @@
-#ifndef __BASE_SAMPLES_RIGID_BODY_STATE_HH
-#define __BASE_SAMPLES_RIGID_BODY_STATE_HH
-
-#include
-#include
-#include
-
-#include
-#include
-
-namespace base { namespace samples {
- /** Representation of the state of a rigid body
- *
- * This is among other things used to express frame transformations by
- * Rock's transformer
- *
- * Given a source and target frame, this structure expresses the _frame
- * change_ between these two frames. In effect, it represents the state of
- * the source frame expressed in the target frame.
- *
- * Per [Rock's conventions](http://rock.opendfki.de/wiki/WikiStart/Standards), you
- * should use a X-forward, right handed coordinate system when assigning
- * frames to bodies (i.e. X=forward, Y=left, Z=up). In addition,
- * world-fixed frames should be aligned to North (North-West-Up, aka NWU)
- *
- * For instance, if sourceFrame is "body" and targetFrame is "world", then
- * the RigidBodyState object is the state of body in the world frame
- * (usually, the world frame has an arbitrary origin and a North-West-Up
- * orientation).
- */
- struct RigidBodyState
- {
-
- RigidBodyState(bool doInvalidation=true){
- if(doInvalidation)
- invalidate();
- };
-
- base::Time time;
-
- /** Name of the source reference frame */
- std::string sourceFrame;
-
- /** Name of the target reference frame */
- std::string targetFrame;
-
- /** Position in m of sourceFrame's origin expressed in targetFrame
- */
- Position position;
- /** Covariance matrix of the position
- */
- base::Matrix3d cov_position;
-
- /** Orientation of targetFrame expressed in sourceFrame */
- Orientation orientation;
- /** Covariance matrix of the orientation as an axis/angle manifold in
- * body coordinates
- */
- base::Matrix3d cov_orientation;
-
- /** Velocity in m/s of sourceFrame expressed in targetFrame */
- base::Vector3d velocity;
- /** Covariance of the velocity
- */
- base::Matrix3d cov_velocity;
-
- /** Angular Velocity as an axis-angle representation in body fixed frame
- *
- * The direction of the vector is the axis, its length the speed */
- base::Vector3d angular_velocity;
- /** Covariance of the angular velocity
- */
- base::Matrix3d cov_angular_velocity;
-
- void setTransform(const Eigen::Affine3d& transform)
- {
- position = transform.translation();
- orientation = Eigen::Quaterniond( transform.linear() );
- }
-
- Eigen::Affine3d getTransform() const
- {
- Eigen::Affine3d ret;
- ret.setIdentity();
- ret.rotate(this->orientation);
- ret.translation() = this->position;
- return ret;
- }
-
- void setPose(const base::Pose& pose)
- {
- orientation = pose.orientation;
- position = pose.position;
- }
-
- base::Pose getPose() const
- {
- return base::Pose( position, orientation );
- }
-
- double getYaw() const
- {
- return base::getYaw(orientation);
- }
-
- double getPitch() const
- {
- return base::getPitch(orientation);
- }
-
- double getRoll() const
- {
- return base::getRoll(orientation);
- }
-
- template
- operator Eigen::Transform() const
- {
- Eigen::Transform ret;
- ret.setIdentity();
- ret.rotate(this->orientation);
- ret.translation() = this->position;
- return ret;
- }
-
- static RigidBodyState unknown(){
- RigidBodyState result(false);
- result.initUnknown();
- return result;
- };
-
- static RigidBodyState invalid() {
- RigidBodyState result(true);
- return result;
- };
-
- /** For backward compatibility only. Use invalidate() */
- void initSane() {
- invalidate();
- }
-
- /** Initializes the rigid body state with NaN for the
- * position, velocity, orientation and angular velocity.
- */
- void invalidate() {
- invalidateOrientation();
- invalidateOrientationCovariance();
- invalidatePosition();
- invalidatePositionCovariance();
- invalidateVelocity();
- invalidateVelocityCovariance();
- invalidateAngularVelocity();
- invalidateAngularVelocityCovariance();
- }
-
- /**
- * Initializes the rigid body state unknown with Zero for the
- * position, velocity and angular velocity, Identity for the orientation
- * and infinity for all covariances.
- */
- void initUnknown()
- {
- position.setZero();
- velocity.setZero();
- orientation = Eigen::Quaterniond::Identity();
- angular_velocity.setZero();
- cov_position = setValueUnknown();
- cov_orientation = setValueUnknown();
- cov_velocity = setValueUnknown();
- cov_angular_velocity = setValueUnknown();
- }
-
- /** Helper method that checks if a value is valid (not NaN anywhere). */
- static bool isValidValue(base::Vector3d const& vec)
- {
- return !base::isNaN(vec(0)) &&
- !base::isNaN(vec(1)) &&
- !base::isNaN(vec(2));
- }
-
- /** Helper method that checks if an orientation is valid (not NaN anywhere)
- * and that the orientation is an unit quaternion. */
- static bool isValidValue(base::Orientation const& ori)
- {
- return !base::isNaN(ori.w()) &&
- !base::isNaN(ori.x()) &&
- !base::isNaN(ori.y()) &&
- !base::isNaN(ori.z()) &&
- fabs(ori.squaredNorm()-1.0) < 1e-6; //assuming at least single precision
- }
-
- /** Helper method that checks if the value whose covariance is
- * represented by the given matrix is a known value (not Inf in the diagonal).
- */
- static bool isKnownValue(base::Matrix3d const& cov)
- {
- return !base::isInfinity(cov(0,0)) &&
- !base::isInfinity(cov(1,1)) &&
- !base::isInfinity(cov(2,2));
- }
-
- /** Helper method that checks if the covariance represented by the given
- * matrix is valid
- */
- static bool isValidCovariance(base::Matrix3d const& cov)
- {
- for (int i = 0; i < 3; ++i)
- for (int j = 0; j < 3; ++j)
- if (base::isNaN(cov(i, j)))
- return false;
- return true;
- }
-
- /** Checks if a given dimension of a vector is valid. */
- static bool isValidValue(base::Vector3d const& vec, int dim)
- {
- return !base::isNaN(vec(dim));
- }
-
- /** Checks if a given dimension of a covariance matrix is valid. */
- static bool isValidCovariance(base::Matrix3d const& cov, int dim)
- {
- return !base::isNaN(cov(dim,dim));
- }
-
- /** Checks if the dimension of a vector given by a covariance is known. */
- static bool isKnownValue(base::Matrix3d const& cov, int dim)
- {
- return !base::isInfinity(cov(dim,dim));
- }
-
- static base::Vector3d invalidValue()
- {
- return base::Vector3d::Ones() * base::NaN();
- }
-
- static base::Orientation invalidOrientation()
- {
- return base::Orientation(Eigen::Vector4d::Ones() * base::NaN());
- }
-
- static base::Matrix3d setValueUnknown()
- {
- return base::Matrix3d::Ones() * base::infinity();
- }
-
- static base::Matrix3d invalidCovariance()
- {
- return base::Matrix3d::Ones() * base::NaN();
- }
-
- bool hasValidPosition() const { return isValidValue(position); }
- bool hasValidPosition(int idx) const { return isValidValue(position, idx); }
- bool hasValidPositionCovariance() const { return isValidCovariance(cov_position); }
- void invalidatePosition() { position = invalidValue(); }
- void invalidatePositionCovariance() { cov_position = invalidCovariance(); }
-
- bool hasValidOrientation() const { return isValidValue(orientation); }
- bool hasValidOrientationCovariance() const { return isValidCovariance(cov_orientation); }
- void invalidateOrientation() { orientation = invalidOrientation(); }
- void invalidateOrientationCovariance() { cov_orientation = invalidCovariance(); }
-
- bool hasValidVelocity() const { return isValidValue(velocity); }
- bool hasValidVelocity(int idx) const { return isValidValue(velocity, idx); }
- bool hasValidVelocityCovariance() const { return isValidCovariance(cov_velocity); }
- void invalidateVelocity() { velocity = invalidValue(); }
- void invalidateVelocityCovariance() { cov_velocity = invalidCovariance(); }
-
- bool hasValidAngularVelocity() const { return isValidValue(angular_velocity); }
- bool hasValidAngularVelocity(int idx) const { return isValidValue(angular_velocity, idx); }
- bool hasValidAngularVelocityCovariance() const { return isValidCovariance(cov_angular_velocity); }
- void invalidateAngularVelocity() { angular_velocity = invalidValue(); }
- void invalidateAngularVelocityCovariance() { cov_angular_velocity = invalidCovariance(); }
-
- void invalidateValues(bool invPos, bool invOri, bool invVel = true,
- bool invAngVel = true) {
- if (invPos) invalidatePosition();
- if (invOri) invalidateOrientation();
- if (invVel) invalidateVelocity();
- if (invAngVel) invalidateAngularVelocity();
- }
-
- void invalidateCovariances(bool invPos = true, bool invOri = true,
- bool invVel = true, bool invAngVel = true) {
- if (invPos) invalidatePositionCovariance();
- if (invOri) invalidateOrientationCovariance();
- if (invVel) invalidateVelocityCovariance();
- if (invAngVel) invalidateAngularVelocityCovariance();
- }
- };
-}}
-
-#endif
diff --git a/base/samples/SonarBeam.hpp b/base/samples/SonarBeam.hpp
deleted file mode 100644
index 1ca76a8f..00000000
--- a/base/samples/SonarBeam.hpp
+++ /dev/null
@@ -1,103 +0,0 @@
-#ifndef BASE_SAMPLES_SONARBEAM_H__
-#define BASE_SAMPLES_SONARBEAM_H__
-
-#include
-#include
-#include
-#include
-#include
-
-namespace base { namespace samples {
-
- struct SonarBeam {
- typedef boost::uint8_t uint8_t;
-
- //timestamp of the sonar beam
- Time time;
-
- //direction of the sonar beam in radians [-pi,+pi]
- //zero is at the front
- Angle bearing;
-
- //sampling interval of each range bin in secs
- double sampling_interval;
-
- //speed of sound
- //this takes the medium into account
- float speed_of_sound;
-
- //horizontal beamwidth of the sonar beam in radians
- float beamwidth_horizontal;
-
- //vertical beamwidth of the sonar beam in radians
- float beamwidth_vertical;
-
- //received echoes (bins) along the beam
- std::vector beam;
-
- SonarBeam():
- sampling_interval(std::numeric_limits::signaling_NaN()),
- speed_of_sound(std::numeric_limits::signaling_NaN()),
- beamwidth_horizontal(std::numeric_limits::signaling_NaN()),
- beamwidth_vertical(std::numeric_limits::signaling_NaN()){}
-
- SonarBeam(const SonarBeam &other)
- {
- init(other);
- }
-
- //calculates the spatial resolution of the sonar beam in meter
- //this takes the sampling_interval and the speed of sound into account
- double getSpatialResolution()const
- {
- //the sampling interval includes the time for
- //the sound traveling from the transiter to the target an back
- //to receiver
- return sampling_interval*0.5*speed_of_sound;
- }
-
- SonarBeam &operator=(const SonarBeam &other)
- {
- init(other);
- return *this;
- }
-
- void init(const SonarBeam &other)
- {
- time = other.time;
- bearing = other.bearing;
- sampling_interval = other.sampling_interval;
- speed_of_sound = other.speed_of_sound;
- beamwidth_horizontal = other.beamwidth_horizontal;
- beamwidth_vertical = other.beamwidth_vertical;
- beam = other.beam;
- }
-
- void swap(SonarBeam &other)
- {
- Time temp_time = time;
- Angle temp_bearing = bearing;
- double temp_sampling_interval = sampling_interval;
- float temp_speed_of_sound = speed_of_sound;
- float temp_beamwidth_horizontal = beamwidth_horizontal;
- float temp_beamwidth_vertical= beamwidth_vertical;
-
- time = other.time;
- bearing = other.bearing;
- sampling_interval = other.sampling_interval;
- speed_of_sound = other.speed_of_sound;
- beamwidth_horizontal = other.beamwidth_horizontal;
- beamwidth_vertical = other.beamwidth_vertical;
- beam.swap(other.beam);
-
- other.time = temp_time;
- other.bearing = temp_bearing;
- other.sampling_interval = temp_sampling_interval;
- other.speed_of_sound = temp_speed_of_sound;
- other.beamwidth_horizontal = temp_beamwidth_horizontal;
- other.beamwidth_vertical = temp_beamwidth_vertical;
- }
- };
-}}
-
-#endif
diff --git a/base/samples/SonarScan.hpp b/base/samples/SonarScan.hpp
deleted file mode 100644
index 5de61a8f..00000000
--- a/base/samples/SonarScan.hpp
+++ /dev/null
@@ -1,398 +0,0 @@
-/*! \file sonar_scan.h
- \brief container for sonar scan data
- */
-
-#ifndef BASE_SAMPLES_SONARSCAN_H__
-#define BASE_SAMPLES_SONARSCAN_H__
-
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include
-#include
-#include
-
-namespace base { namespace samples {
-
- struct SonarScan
- {
- public:
- /**
- * Initialize the sonar scan
- */
- SonarScan()
- : number_of_beams(0)
- , number_of_bins(0)
- , sampling_interval(0)
- , speed_of_sound(0)
- , beamwidth_horizontal(base::Angle::fromRad(0))
- , beamwidth_vertical(base::Angle::fromRad(0))
- , memory_layout_column(true)
- , polar_coordinates(true)
- {
- reset();
- }
-
- SonarScan(uint16_t number_of_beams,uint16_t number_of_bins,Angle start_bearing,Angle angular_resolution,bool memory_layout_column=true)
- {
- init(number_of_beams,number_of_bins,start_bearing,angular_resolution,memory_layout_column);
- }
-
- //makes a copy of other
- SonarScan(const SonarScan &other,bool bcopy = true)
- {
- init(other,bcopy);
- }
-
- SonarScan &operator=(const SonarScan &other)
- {
- init(other,true);
- return *this;
- }
-
- //makes a copy of other
- void init(const SonarScan &other,bool bcopy = true)
- {
- init(other.number_of_beams,other.number_of_bins,other.start_bearing,other.angular_resolution,other.memory_layout_column);
- time = other.time;
-
- beamwidth_vertical = other.beamwidth_vertical;
- beamwidth_horizontal = other.beamwidth_horizontal;
- sampling_interval = other.sampling_interval;
- speed_of_sound = other.speed_of_sound;
- polar_coordinates = other.polar_coordinates;
- if(bcopy){
- setData(other.getData());
- time_beams = other.time_beams;
- }
- }
-
- void init(uint16_t number_of_beams, uint16_t number_of_bins, Angle start_bearing, Angle angular_resolution,bool memory_layout_column = true, int val=-1)
- {
- //change size if the sonar scan does not fit
- if(this->number_of_beams != number_of_beams || this->number_of_bins != number_of_bins)
- {
- this->number_of_beams = number_of_beams;
- this->number_of_bins = number_of_bins;
- data.resize(number_of_beams*number_of_bins);
- }
- this->start_bearing = start_bearing;
- this->angular_resolution = angular_resolution;
- this->memory_layout_column = memory_layout_column;
- speed_of_sound = 0;
- beamwidth_horizontal = base::Angle::fromRad(0);
- beamwidth_vertical = base::Angle::fromRad(0);
- reset(val);
- }
-
- // if val is negative the sonar scan data will not be initialized
- void reset(int const val = 0)
- {
- this->time = base::Time();
- if (this->data.size() > 0 && val >= 0)
- {
- memset(&this->data[0], val%256, this->data.size());
- }
- time_beams.clear();
- }
-
- //returns the index/column of the sonar beam which
- //holds information about the given bearing
- //returns -1 if the sonar scan has no sonar beam for the given bearing
- int beamIndexForBearing(const Angle bearing,bool range_check=true)const
- {
- double temp_rad = (start_bearing-bearing).rad;
- int index = round(temp_rad/angular_resolution.rad);
- if(range_check && (index < 0 || index >= number_of_beams))
- return -1;
- return index;
- }
-
- //returns true if a sonar beam was already added for the given
- //bearing
- //internally time_beams is used for checking
- bool hasSonarBeam(const base::samples::SonarBeam &sonar_beam)const
- {
- return hasSonarBeam(sonar_beam.bearing);
- }
-
- bool hasSonarBeam(const Angle bearing)const
- {
- int index = beamIndexForBearing(bearing);
- if(index < 0)
- return false;
-
- //it is assumed that all data are set at once (imaging sonar)
- if(time_beams.empty())
- return true;
-
- //it is assumed that the data are set beam by beam (scanning sonar)
- if(time_beams[index].microseconds != 0)
- return true;
- return false;
- }
-
-
- //adds a sonar beam to the sonar scan
- //throws an exception if the sonar scan cannot hold the sonar beam and resize is set to false
- //otherwise the sonar scan will be resized (the start angle is not changed!)
- //
- //the memory layout must be one sonar beam per row otherwise the function will throw a std::runtime_error
- void addSonarBeam(const base::samples::SonarBeam &sonar_beam,bool resize=true)
- {
- if(memory_layout_column)
- throw std::runtime_error("addSonarBeam: cannot add sonar beam because the memory layout is not supported. Call toggleMemoryLayout()");
-
- if(number_of_bins < sonar_beam.beam.size())
- throw std::runtime_error("addSonarBeam: cannot add sonar beam: too many bins");
-
- int index = beamIndexForBearing(sonar_beam.bearing,false);
- if(index < 0)
- throw std::runtime_error("addSonarBeam: negative index!");
- if(index >= number_of_beams)
- {
- if(!resize)
- throw std::runtime_error("addSonarBeam: bearing is out of range");
- number_of_beams = index+1;
- data.resize(number_of_beams*number_of_bins);
- }
- if(time_beams.size() != number_of_beams)
- time_beams.resize(number_of_beams);
-
- time_beams[index] = sonar_beam.time;
- sampling_interval = sonar_beam.sampling_interval;
- beamwidth_vertical = Angle::fromRad(sonar_beam.beamwidth_vertical);
- beamwidth_horizontal = Angle::fromRad(sonar_beam.beamwidth_horizontal);
- speed_of_sound = sonar_beam.speed_of_sound;
- memcpy(&data[index*number_of_bins],&sonar_beam.beam[0],sonar_beam.beam.size());
- }
-
- //returns a sonar beam for a given bearing
- //throws an exception if the sonar scans holds no information for the given bearing
- //the memory layout must be one beam per row
- //
- //an exception is thrown if the sonar beam holds no information about the given bearing
- void getSonarBeam(const Angle bearing,SonarBeam &sonar_beam)const
- {
- if(memory_layout_column)
- throw std::runtime_error("getSonarBeam: Wrong memory layout!");
- int index = beamIndexForBearing(bearing);
- if(index<0)
- throw std::runtime_error("getSonarBeam: No Data for the given bearing!");
-
- sonar_beam.beam.resize(number_of_bins);
- memcpy(&sonar_beam.beam[0],&data[number_of_bins*index],number_of_bins);
- if((int)time_beams.size() > index)
- sonar_beam.time = time_beams[index];
- else
- sonar_beam.time = time;
- sonar_beam.speed_of_sound = speed_of_sound;
- sonar_beam.beamwidth_horizontal = beamwidth_horizontal.rad;
- sonar_beam.beamwidth_vertical = beamwidth_vertical.rad;
- sonar_beam.sampling_interval = sampling_interval;
- sonar_beam.bearing = bearing;
- }
-
- //this toggles the memory layout between one sonar beam per row and one sonar beam per column
- //to add sonar beams the memory layout must be one sonar beam per row
- void toggleMemoryLayout()
- {
- std::vector temp;
- temp.resize(data.size());
-
- if(memory_layout_column)
- {
- for(int row=0;row < number_of_beams;++row)
- for(int column=0;column < number_of_bins;++column)
- temp[row*number_of_bins+column] =data[column*number_of_beams+row];
- }
- else
- {
- for(int row=0;row < number_of_beams;++row)
- for(int column=0;column < number_of_bins;++column)
- temp[column*number_of_beams+row] =data[row*number_of_bins+column];
- }
- memory_layout_column = !memory_layout_column;
- data.swap(temp);
- }
-
- void swap(SonarScan &sonar_scan)
- {
- //swap vector
- data.swap(sonar_scan.data);
-
- //copy values to temp
- base::Time temp_time = sonar_scan.time;
- Angle temp_beamwidth_vertical = sonar_scan.beamwidth_vertical;
- Angle temp_beamwidth_horizontal = sonar_scan.beamwidth_horizontal;
- double temp_sampling_interval = sonar_scan.sampling_interval;
- uint16_t temp_number_of_beams = sonar_scan.number_of_beams;
- uint16_t temp_number_of_bins = sonar_scan.number_of_bins;
- Angle temp_start_bearing = sonar_scan.start_bearing;
- Angle temp_angular_resolution = sonar_scan.angular_resolution;
- bool temp_memory_layout_column = sonar_scan.memory_layout_column;
- bool temp_polar_coordinates = sonar_scan.polar_coordinates;
- float temp_speed_of_sound = sonar_scan.speed_of_sound;
-
- //copy values
- sonar_scan.time = time;
- sonar_scan.beamwidth_vertical = beamwidth_vertical;
- sonar_scan.beamwidth_horizontal = beamwidth_horizontal;
- sonar_scan.sampling_interval = sampling_interval;
- sonar_scan.number_of_beams = number_of_beams;
- sonar_scan.number_of_bins = number_of_bins;
- sonar_scan.start_bearing = start_bearing;
- sonar_scan.angular_resolution = angular_resolution;
- sonar_scan.memory_layout_column = memory_layout_column;
- sonar_scan.polar_coordinates = polar_coordinates;
- sonar_scan.speed_of_sound = speed_of_sound;
-
- time = temp_time;
- beamwidth_vertical = temp_beamwidth_vertical;
- beamwidth_horizontal = temp_beamwidth_horizontal;
- sampling_interval = temp_sampling_interval;
- number_of_beams = temp_number_of_beams;
- number_of_bins = temp_number_of_bins;
- start_bearing = temp_start_bearing;
- angular_resolution = temp_angular_resolution;
- memory_layout_column = temp_memory_layout_column;
- polar_coordinates = temp_polar_coordinates;
- speed_of_sound = temp_speed_of_sound;
- }
-
- inline uint32_t getNumberOfBytes() const {
- return data.size();
- }
-
- /**
- * Returns the total count of bins in this sonar scan
- * @return Returns the overall number of bins (number_of_beams * number_of_bins)
- */
- inline uint32_t getBinCount() const {
- return number_of_beams * number_of_bins;
- }
-
- inline const std::vector &getData() const {
- return this->data;
- }
-
-
- Angle getEndBearing()const
- {
- return start_bearing-angular_resolution*(number_of_beams-1);
- }
-
- Angle getStartBearing()const
- {
- return start_bearing;
- }
-
- Angle getAngularResolution()const
- {
- return angular_resolution;
- }
-
- //calculates the spatial resolution of the sonar scan in meter
- //this takes the sampling_interval and the speed of sound into account
- double getSpatialResolution()const
- {
- //the sampling interval includes the time for
- //the sound traveling from the transmitter to the target an back
- //to the receiver
- return sampling_interval*0.5*speed_of_sound;
- }
-
- inline void setData(const std::vector &data) {
- this->data = data;
- }
- inline void setData(const char *data, uint32_t size) {
- if (size != this->data.size())
- {
- std::cerr << "SonarScan: "
- << __FUNCTION__ << " (" << __FILE__ << ", line "
- << __LINE__ << "): " << "size mismatch in setData() ("
- << size << " != " << this->data.size() << ")"
- << std::endl;
- return;
- }
- memcpy(&this->data[0], data, size);
- }
-
- inline uint8_t *getDataPtr() {
- return static_cast(&this->data[0]);
- }
- inline const uint8_t *getDataConstPtr() const {
- return static_cast(&this->data[0]);
- }
-
- //The time at which this sonar scan has been captured
- base::Time time;
-
- //The raw data of the sonar scan
- std::vector data;
-
- //Time stamp for each sonar beam
- //if the time stamp is 1 January 1970
- //the beam is regarded as not be set
- //vector can be empty in this case time
- //is used for all beams
- std::vector time_beams;
-
- //number of beams
- //this can be interpreted as image width
- uint16_t number_of_beams;
-
- //number of bins
- //this can be interpreted as image height
- uint16_t number_of_bins;
-
- /** The angle at which the reading starts. Zero is at the front of
- * the device and turns counter-clockwise.
- * This value is in radians
- *
- * All beams are stored from left to right to match the memory
- * layout of an image !!! Therefore the end bearing is usually
- * smaller than the start bearing
- */
- Angle start_bearing;
-
- // Angle difference between two beams in radians
- // The beams are stored from left to right
- // to match the memory layout of an image
- //
- // This value must be always positive !
- Angle angular_resolution;
-
- //sampling interval of each range bin in secs
- double sampling_interval;
-
- //speed of sound
- //this takes the medium into account
- float speed_of_sound;
-
- //horizontal beam width of the sonar beam in radians
- Angle beamwidth_horizontal;
-
- //vertical beam width of the sonar beam in radians
- Angle beamwidth_vertical;
-
- //if set to true one sonar beam is stored per column
- //otherwise per row
- bool memory_layout_column;
-
- //if set to true the bins are interpreted in polar coordinates
- //otherwise in Cartesian coordinates
- //
- //Some imaging sonars store their data in Cartesian rather than
- //polar coordinates (BlueView)
- bool polar_coordinates;
- };
-}}
-
-#endif
-
diff --git a/bindings/ruby/CMakeLists.txt b/bindings/ruby/CMakeLists.txt
index 84eeed0f..354bf24d 100644
--- a/bindings/ruby/CMakeLists.txt
+++ b/bindings/ruby/CMakeLists.txt
@@ -17,7 +17,7 @@ if (base_AVAILABLE)
endif(SISL_FOUND)
install(FILES lib/eigen.rb
DESTINATION ${RUBY_LIBRARY_INSTALL_DIR})
- target_link_libraries(base_types_ruby base)
+ target_link_libraries(base_types_ruby base-types)
rock_typelib_ruby_plugin(lib/base/typelib_plugin.rb RENAME base_types.rb)
endif()
diff --git a/src/Angle.cpp b/src/Angle.cpp
new file mode 100644
index 00000000..6f0a4f4d
--- /dev/null
+++ b/src/Angle.cpp
@@ -0,0 +1,175 @@
+#include "Angle.hpp"
+#include
+#include
+
+namespace base {
+
+Angle Angle::vectorToVector(const Vector3d& a, const Vector3d& b)
+{
+ double dot = a.dot(b);
+ double norm = a.norm() * b.norm();
+ return fromRad(acos(dot / norm));
+}
+
+Angle Angle::vectorToVector(const Vector3d& a, const Vector3d& b, const Vector3d& positive)
+{
+ double cos = a.dot(b) / (a.norm() * b.norm());
+
+ bool is_positive = (a.cross(b).dot(positive) > 0);
+ if (is_positive)
+ return fromRad(acos(cos));
+ else
+ return fromRad(-acos(cos));
+}
+
+std::ostream& operator << (std::ostream& os, Angle angle)
+{
+ os << angle.getRad() << boost::format("[%3.1fdeg]") % angle.getDeg();
+ return os;
+}
+
+AngleSegment::AngleSegment(): width(0), startRad(0), endRad(0)
+{
+}
+
+AngleSegment::AngleSegment(const Angle &start, double _width): width(_width), startRad(start.getRad()), endRad(startRad + width)
+{
+ if(width < 0)
+ throw std::runtime_error("Error got segment with negative width");
+}
+
+bool AngleSegment::isInside(const Angle& angle) const
+{
+ double angleRad = angle.getRad();
+ if(angleRad < startRad)
+ angleRad += 2*M_PI;
+
+ if(angleRad <= endRad) //startRad <= angleRad &&
+ return true;
+
+ return false;
+}
+
+bool AngleSegment::isInside(const AngleSegment& segment) const
+{
+ double otherStart = segment.startRad;
+ if(otherStart < startRad)
+ otherStart += 2*M_PI;
+
+ double otherEnd = otherStart + segment.width;
+
+ if(otherEnd <= endRad)
+ return true;
+
+ return false;
+}
+
+std::vector< AngleSegment > AngleSegment::getIntersections(const AngleSegment& b) const
+{
+ std::vector ret;
+ //special case, this segment is a whole circle
+ if(width >= 2*M_PI)
+ {
+ ret.push_back(b);
+ return ret;
+ }
+
+ //special case, other segment is a whole circle
+ if(b.width >= 2*M_PI)
+ {
+ ret.push_back(*this);
+ return ret;
+ }
+
+ double startA = startRad;
+ double startB = b.startRad;
+ double widthA = width;
+ double widthB = b.width;
+
+ //make A the smaller angle
+ if(startA > startB)
+ {
+ std::swap(startA, startB);
+ std::swap(widthA, widthB);
+ }
+ double endA = startA + widthA;
+ double endB = startB + widthB;
+
+ //test if segemnts do not intersect at all
+ if(endA < startB)
+ {
+ //wrap case
+ if(endB > M_PI)
+ {
+ //check if segments intersect after wrap correction
+ if(startA < endB - 2*M_PI)
+ {
+ //this means the start of A is inside of B
+ //drop first part of B and realign it to -M_PI
+ //also switch A and B as B is now the 'lower' one
+ double newWidthA = widthB - (M_PI - startB);
+ startB = startA;
+ widthB = widthA;
+ startA = - M_PI;
+ widthA = newWidthA;
+ endA = startA + widthA;
+ endB = startB + widthB;
+ //no return, still need
+ //to check for intersection
+ }
+ else
+ //no intersection
+ return ret;
+ } else
+ //no intersection
+ return ret;
+ }
+
+ //normal case, no wrap around
+ double newStart = startB;
+ double newEnd = 0;
+
+ if(endA < endB)
+ {
+ newEnd = endA;
+ }
+ else
+ {
+ newEnd = endB;
+ }
+
+ double newWidth = newEnd - newStart;
+
+ //filter invalid segments
+ if(newWidth > 1e-10)
+ ret.push_back(AngleSegment(Angle::fromRad(newStart), newWidth));
+
+ newStart = endB - 2*M_PI;
+ if(newStart > startA)
+ {
+ newWidth = newStart - startA;
+ //filter invalid segments
+ if(newWidth > 1e-10)
+ ret.push_back(AngleSegment(Angle::fromRad(startA), newWidth));
+ }
+
+ return ret;
+}
+
+Angle AngleSegment::getStart() const
+{
+ return Angle::fromRad(startRad);
+}
+
+Angle AngleSegment::getEnd() const
+{
+ return Angle::fromRad(endRad);
+}
+
+std::ostream& operator << (std::ostream& os, AngleSegment seg)
+{
+ os << " Segmend start " << seg.startRad/M_PI *180.0 << " end " << seg.endRad/M_PI * 180.0 << " width " << seg.width /M_PI * 180.0;
+ return os;
+}
+
+} //end namespace base
diff --git a/base/Angle.hpp b/src/Angle.hpp
similarity index 58%
rename from base/Angle.hpp
rename to src/Angle.hpp
index 158aef5e..2fa3e798 100644
--- a/base/Angle.hpp
+++ b/src/Angle.hpp
@@ -1,12 +1,12 @@
#ifndef __BASE_ANGLE_HH__
#define __BASE_ANGLE_HH__
-#include
#include
-#include
#include
-#include
#include
+#include
+
+#include
namespace base
{
@@ -203,23 +203,6 @@ class Angle
return Angle::fromRad( getRad() * val );
}
-BASE_TYPES_DEPRECATED_SUPPRESS_START
- /**
- * @deprecated this function does not work is right_limit == left_limit.
- * From the API one can't differenciate between small interval
- * or a full cycle. Use base::AngleSegment instead
- * @return true if the angle is insight the given interval
- */
- bool isInRange(const Angle& left_limit, const Angle& right_limit) const BASE_TYPES_DEPRECATED
- {
- if((right_limit-left_limit).rad < 0)
- return !isInRange(right_limit,left_limit);
- if((*this -left_limit).getRad() >= 0 && (right_limit -*this).getRad() >= 0)
- return true;
- return false;
- }
-BASE_TYPES_DEPRECATED_SUPPRESS_STOP
-
/**
* Returns a new angle which is the inverse of tis object.
* */
@@ -242,12 +225,8 @@ BASE_TYPES_DEPRECATED_SUPPRESS_STOP
/** Computes the unsigned angle of the rotation that makes +a+ colinear with +b+
*/
- static Angle vectorToVector(const base::Vector3d& a, const base::Vector3d& b)
- {
- double dot = a.dot(b);
- double norm = a.norm() * b.norm();
- return fromRad(acos(dot / norm));
- }
+ static Angle vectorToVector(const base::Vector3d& a, const base::Vector3d& b);
+
/** Computes the angle of the rotation that makes +a+ colinear with +b+
*
@@ -255,16 +234,7 @@ BASE_TYPES_DEPRECATED_SUPPRESS_STOP
* @c positive vector defining the positive rotation direction. @c positive
* is required to be of unit length.
*/
- static Angle vectorToVector(const base::Vector3d& a, const base::Vector3d& b, const base::Vector3d& positive)
- {
- double cos = a.dot(b) / (a.norm() * b.norm());
-
- bool is_positive = (a.cross(b).dot(positive) > 0);
- if (is_positive)
- return fromRad(acos(cos));
- else
- return fromRad(-acos(cos));
- }
+ static Angle vectorToVector(const base::Vector3d& a, const base::Vector3d& b, const base::Vector3d& positive);
};
static inline Angle operator*( double a, Angle b )
@@ -272,11 +242,7 @@ static inline Angle operator*( double a, Angle b )
return Angle::fromRad( a * b.getRad() );
}
-static inline std::ostream& operator << (std::ostream& os, Angle angle)
-{
- os << angle.getRad() << boost::format("[%3.1fdeg]") % angle.getDeg();
- return os;
-}
+std::ostream& operator << (std::ostream& os, Angle angle);
/**
* This class represents a Segment of a Circle.
@@ -286,51 +252,23 @@ static inline std::ostream& operator << (std::ostream& os, Angle angle)
class AngleSegment
{
public:
- AngleSegment(): width(0), startRad(0), endRad(0)
- {
- }
+ AngleSegment();
- AngleSegment(const Angle &start, double _width): width(_width), startRad(start.getRad()), endRad(startRad + width)
- {
- if(width < 0)
- throw std::runtime_error("Error got segment with negative width");
- }
+ AngleSegment(const Angle &start, double _width);
/**
* Tests if the given angle is inside of the segment.
* @param angle - angle to be tested
* @return true if angle is inside the segment
* */
- bool isInside(const Angle &angle) const
- {
- double angleRad = angle.getRad();
- if(angleRad < startRad)
- angleRad += 2*M_PI;
-
- if(angleRad <= endRad) //startRad <= angleRad &&
- return true;
-
- return false;
- };
+ bool isInside(const Angle &angle) const;
/**
* Tests if the given segment is inside of this segment.
* @param segment - segment to be tested
* @return true if the given segment is inside of this segment
* */
- bool isInside(const AngleSegment &segment) const
- {
- double otherStart = segment.startRad;
- if(otherStart < startRad)
- otherStart += 2*M_PI;
-
- double otherEnd = otherStart + segment.width;
-
- if(otherEnd <= endRad)
- return true;
-
- return false;
- };
+ bool isInside(const AngleSegment &segment) const;
bool split(const Angle &angle, AngleSegment &rest)
{
@@ -347,97 +285,7 @@ class AngleSegment
* @param b The segment wich schould be tested
* @return A vector, containing a segment for each intersecting part of the segments.
* */
- std::vector getIntersections(const AngleSegment &b) const
- {
- std::vector ret;
- //special case, this segment is a whole circle
- if(width >= 2*M_PI)
- {
- ret.push_back(b);
- return ret;
- }
-
- //special case, other segment is a whole circle
- if(b.width >= 2*M_PI)
- {
- ret.push_back(*this);
- return ret;
- }
-
- double startA = startRad;
- double startB = b.startRad;
- double widthA = width;
- double widthB = b.width;
-
- //make A the smaller angle
- if(startA > startB)
- {
- std::swap(startA, startB);
- std::swap(widthA, widthB);
- }
- double endA = startA + widthA;
- double endB = startB + widthB;
-
- //test if segemnts do not intersect at all
- if(endA < startB)
- {
- //wrap case
- if(endB > M_PI)
- {
- //check if segments intersect after wrap correction
- if(startA < endB - 2*M_PI)
- {
- //this means the start of A is inside of B
- //drop first part of B and realign it to -M_PI
- //also switch A and B as B is now the 'lower' one
- double newWidthA = widthB - (M_PI - startB);
- startB = startA;
- widthB = widthA;
- startA = - M_PI;
- widthA = newWidthA;
- endA = startA + widthA;
- endB = startB + widthB;
- //no return, still need
- //to check for intersection
- }
- else
- //no intersection
- return ret;
- } else
- //no intersection
- return ret;
- }
-
- //normal case, no wrap around
- double newStart = startB;
- double newEnd = 0;
-
- if(endA < endB)
- {
- newEnd = endA;
- }
- else
- {
- newEnd = endB;
- }
-
- double newWidth = newEnd - newStart;
-
- //filter invalid segments
- if(newWidth > 1e-10)
- ret.push_back(AngleSegment(Angle::fromRad(newStart), newWidth));
-
- newStart = endB - 2*M_PI;
- if(newStart > startA)
- {
- newWidth = newStart - startA;
- //filter invalid segments
- if(newWidth > 1e-10)
- ret.push_back(AngleSegment(Angle::fromRad(startA), newWidth));
- }
-
- return ret;
- }
+ std::vector getIntersections(const AngleSegment &b) const;
/**
* Returns the width of the segment in radians
@@ -452,10 +300,7 @@ class AngleSegment
* Returns the start angle of the segement
* @return the start angle of the segement
* */
- base::Angle getStart() const
- {
- return base::Angle::fromRad(startRad);
- }
+ base::Angle getStart() const;
/**
* Returns the end angle of the segement
@@ -465,10 +310,7 @@ class AngleSegment
* getStart + getWidth;
* @return the end angle of the segement
* */
- base::Angle getEnd() const
- {
- return base::Angle::fromRad(endRad);
- }
+ base::Angle getEnd() const;
/**
* Widht of the segment in radians
@@ -486,11 +328,7 @@ class AngleSegment
double endRad;
};
-static inline std::ostream& operator << (std::ostream& os, AngleSegment seg)
-{
- os << " Segmend start " << seg.startRad/M_PI *180.0 << " end " << seg.endRad/M_PI * 180.0 << " width " << seg.width /M_PI * 180.0;
- return os;
-}
+std::ostream& operator << (std::ostream& os, AngleSegment seg);
}
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 408508cf..934dbaaf 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,25 +1,96 @@
-set(SOURCES )
+rock_library(
+ base-types
+ Angle.cpp
+ JointLimitRange.cpp
+ JointLimits.cpp
+ JointState.cpp
+ JointsTrajectory.cpp
+ JointTransform.cpp
+ Pose.cpp
+ Pressure.cpp
+ Spline.cpp
+ Temperature.cpp
+ Time.cpp
+ TimeMark.cpp
+ Timeout.cpp
+ Trajectory.cpp
+ TransformWithCovariance.cpp
+ TwistWithCovariance.cpp
+ Waypoint.cpp
+ commands/Motion2D.cpp
+ samples/BodyState.cpp
+ samples/DepthMap.cpp
+ samples/DistanceImage.cpp
+ samples/Frame.cpp
+ samples/Joints.cpp
+ samples/LaserScan.cpp
+ samples/Pressure.cpp
+ samples/RigidBodyAcceleration.cpp
+ samples/RigidBodyState.cpp
+ samples/Sonar.cpp
+ samples/SonarBeam.cpp
+ samples/SonarScan.cpp
+ HEADERS
+ Angle.hpp
+ CircularBuffer.hpp
+ Deprecated.hpp
+ Eigen.hpp
+ Float.hpp
+ JointLimitRange.hpp
+ JointLimits.hpp
+ JointState.hpp
+ JointsTrajectory.hpp
+ JointTransform.hpp
+ Logging.hpp
+ Matrix.hpp
+ NamedVector.hpp
+ Point.hpp
+ Pose.hpp
+ Pressure.hpp
+ Singleton.hpp
+ Spline.hpp
+ Temperature.hpp
+ Time.hpp
+ TimeMark.hpp
+ Timeout.hpp
+ Trajectory.hpp
+ TransformWithCovariance.hpp
+ TwistWithCovariance.hpp
+ Waypoint.hpp
+ Wrench.hpp
+ commands/Joints.hpp
+ commands/Motion2D.hpp
+ commands/Speed6D.hpp
+ commands/LinearAngular6DCommand.hpp
+ logging/logging_iostream_style.h
+ logging/logging_printf_style.h
+ samples/BodyState.hpp
+ samples/CommandSamples.hpp
+ samples/DepthMap.hpp
+ samples/DistanceImage.hpp
+ samples/Frame.hpp
+ samples/IMUSensors.hpp
+ samples/Joints.hpp
+ samples/LaserScan.hpp
+ samples/Pointcloud.hpp
+ samples/Pressure.hpp
+ samples/RigidBodyAcceleration.hpp
+ samples/RigidBodyState.hpp
+ samples/Sonar.hpp
+ samples/SonarBeam.hpp
+ samples/SonarScan.hpp
+ samples/Wrench.hpp
+ samples/Wrenches.hpp
+ templates/TimeStamped.hpp
+ DEPS_CMAKE
+ SISL
+ DEPS_PKGCONFIG
+ base-logging
+ eigen3
+)
-set(HEADERS Logging.hpp Singleton.hpp logging/logging_iostream_style.h logging/logging_printf_style.h)
-
-
-# Using SISL as optional dependency
-find_package(SISL)
-if(SISL_FOUND)
- rock_library(base ${SOURCES} Spline.cpp
- DEPS_CMAKE SISL
- HEADERS ${HEADERS} Spline.hpp
- DEPS_PKGCONFIG base-logging)
- add_definitions(-DSISL_FOUND)
-else(SISL_FOUND)
- message(STATUS "Compiling ${PROJECT_NAME} without 'spline' support")
- rock_library(base ${SOURCES}
- HEADERS ${HEADERS}
- DEPS_PKGCONFIG base-logging)
-endif(SISL_FOUND)
-
-configure_file(${CMAKE_SOURCE_DIR}/base-lib.pc.in ${CMAKE_BINARY_DIR}/base-lib.pc @ONLY)
-install(FILES ${CMAKE_BINARY_DIR}/base-lib.pc DESTINATION lib/pkgconfig)
install(FILES ${CMAKE_SOURCE_DIR}/src/Spline.hpp
DESTINATION include/base/geometry)
+configure_file(base-lib.pc.in ${CMAKE_BINARY_DIR}/base-lib.pc @ONLY)
+install(FILES ${CMAKE_BINARY_DIR}/base-lib.pc DESTINATION lib/pkgconfig)
diff --git a/base/CircularBuffer.hpp b/src/CircularBuffer.hpp
similarity index 100%
rename from base/CircularBuffer.hpp
rename to src/CircularBuffer.hpp
diff --git a/base/Deprecated.hpp b/src/Deprecated.hpp
similarity index 100%
rename from base/Deprecated.hpp
rename to src/Deprecated.hpp
diff --git a/base/Eigen.hpp b/src/Eigen.hpp
similarity index 69%
rename from base/Eigen.hpp
rename to src/Eigen.hpp
index 613e59a1..246676cd 100644
--- a/base/Eigen.hpp
+++ b/src/Eigen.hpp
@@ -4,10 +4,10 @@
#include
#include
-#include
namespace base
{
+
// We define these typedefs to workaround alignment requirements for normal
// Eigen types. This reduces the amount of knowledge people have to have to
// manipulate these types -- as well as the structures that use them -- and
@@ -53,30 +53,6 @@ namespace base
return isnotnan(x - x);
};
- /**
- * Guarantee Symmetric (semi-) Positive Definite (SPD) matrix.
- * The input matrix must be symmetric already (only the lower triangular part will be considered)
- */
- template
- static typename _MatrixType::PlainObject guaranteeSPD (const _MatrixType &A, const typename _MatrixType::RealScalar& minEig = 0.0)
- {
- typedef typename _MatrixType::PlainObject Matrix;
- typedef Eigen::SelfAdjointEigenSolver EigenDecomp;
- typedef typename EigenDecomp::RealVectorType EigenValues;
-
- // Eigenvalue decomposition
- Eigen::SelfAdjointEigenSolver eigOfA (A, Eigen::ComputeEigenvectors);
-
- // truncate Eigenvalues:
- EigenValues s = eigOfA.eigenvalues();
- s = s.cwiseMax(minEig);
-
- // re-compose matrix:
- Matrix spdA = eigOfA.eigenvectors() * s.asDiagonal() * eigOfA.eigenvectors().adjoint();
-
- return spdA;
- };
-
}
#endif
diff --git a/base/Float.hpp b/src/Float.hpp
similarity index 50%
rename from base/Float.hpp
rename to src/Float.hpp
index fe657902..61ef6b25 100644
--- a/base/Float.hpp
+++ b/src/Float.hpp
@@ -1,17 +1,18 @@
#ifndef BASE_FLOAT_H
#define BASE_FLOAT_H
-#include
+#include
+#include
namespace base {
template T NaN() { return std::numeric_limits::quiet_NaN(); }
- template bool isNaN(T value) { return boost::math::isnan(value); }
+ template bool isNaN(T value) { return std::isnan(value); }
template T unset() { return std::numeric_limits::quiet_NaN(); }
- template bool isUnset(T value) { return boost::math::isnan(value); }
+ template bool isUnset(T value) { return std::isnan(value); }
template T unknown() { return std::numeric_limits::quiet_NaN(); }
- template bool isUnknown(T value) { return boost::math::isnan(value); }
+ template bool isUnknown(T value) { return std::isnan(value); }
template T infinity() { return std::numeric_limits::infinity(); }
- template bool isInfinity(T value) { return boost::math::isinf(value); }
+ template bool isInfinity(T value) { return std::isinf(value); }
}
#endif
diff --git a/src/JointLimitRange.cpp b/src/JointLimitRange.cpp
new file mode 100644
index 00000000..338d94a9
--- /dev/null
+++ b/src/JointLimitRange.cpp
@@ -0,0 +1,118 @@
+#include "JointLimitRange.hpp"
+#include
+
+namespace base
+{
+
+
+std::string JointLimitRange::OutOfBounds::errorString(std::string name, double min, double max, double value)
+{
+ std::ostringstream ss;
+ ss << "The " << name << " value " << value
+ << " was outside the allowed bounds [" << min << ":" << max << "].";
+ return ss.str();
+}
+
+void JointLimitRange::validate(const JointState& state) const
+{
+ std::pair check = verifyValidity(state);
+ if (check.first)
+ throw check.second;
+}
+
+bool JointLimitRange::isValid(const JointState& state) const
+{
+ std::pair check = verifyValidity(state);
+ return check.first;
+}
+
+JointLimitRange JointLimitRange::Position(double min, double max)
+{
+ JointLimitRange result;
+ result.min.position = min;
+ result.max.position = max;
+ return result;
+}
+
+
+JointLimitRange JointLimitRange::Speed(double min, double max)
+{
+ JointLimitRange result;
+ result.min.speed = min;
+ result.max.speed = max;
+ return result;
+}
+
+
+JointLimitRange JointLimitRange::Effort(double min, double max)
+{
+ JointLimitRange result;
+ result.min.effort = min;
+ result.max.effort = max;
+ return result;
+}
+
+JointLimitRange JointLimitRange::Raw(double min, double max)
+{
+ JointLimitRange result;
+ result.min.raw = min;
+ result.max.raw = max;
+ return result;
+}
+
+JointLimitRange JointLimitRange::Acceleration(double min, double max)
+{
+ JointLimitRange result;
+ result.min.acceleration = min;
+ result.max.acceleration = max;
+ return result;
+}
+
+std::pair< bool, JointLimitRange::OutOfBounds > JointLimitRange::verifyValidity(const JointState& state) const
+{
+ using std::make_pair;
+ if( state.hasPosition() )
+ {
+ if( min.hasPosition() && min.position > state.position )
+ return make_pair(false, OutOfBounds( "position", min.position, max.position, state.position ));
+ if( max.hasPosition() && max.position < state.position )
+ return make_pair(false, OutOfBounds( "position", min.position, max.position, state.position ));
+ }
+
+ if( state.hasSpeed() )
+ {
+ if( min.hasSpeed() && min.speed > state.speed )
+ return make_pair(false, OutOfBounds( "speed", min.speed, max.speed, state.speed ));
+ if( max.hasSpeed() && max.speed < state.speed )
+ return make_pair(false, OutOfBounds( "speed", min.speed, max.speed, state.speed ));
+ }
+
+ if( state.hasEffort() )
+ {
+ if( min.hasEffort() && min.effort > state.effort )
+ return make_pair(false, OutOfBounds( "effort", min.effort, max.effort, state.effort ));
+ if( max.hasEffort() && max.effort < state.effort )
+ return make_pair(false, OutOfBounds( "effort", min.effort, max.effort, state.effort ));
+ }
+
+ if( state.hasRaw() )
+ {
+ if( min.hasRaw() && min.raw > state.raw )
+ return make_pair(false, OutOfBounds( "raw", min.raw, max.raw, state.raw ));
+ if( max.hasRaw() && max.raw < state.raw )
+ return make_pair(false, OutOfBounds( "raw", min.raw, max.raw, state.raw ));
+ }
+
+ if( state.hasAcceleration() )
+ {
+ if( min.hasAcceleration() && min.acceleration > state.acceleration )
+ return make_pair(false, OutOfBounds( "acceleration", min.acceleration, max.acceleration, state.acceleration ));
+ if( max.hasAcceleration() && max.acceleration < state.acceleration )
+ return make_pair(false, OutOfBounds( "acceleration", min.acceleration, max.acceleration, state.acceleration ));
+ }
+ return make_pair(true, OutOfBounds());
+}
+
+
+
+} //end namespace base
\ No newline at end of file
diff --git a/src/JointLimitRange.hpp b/src/JointLimitRange.hpp
new file mode 100644
index 00000000..63ce05af
--- /dev/null
+++ b/src/JointLimitRange.hpp
@@ -0,0 +1,79 @@
+#ifndef BASE_JOINT_LIMIT_RANGE_HPP
+#define BASE_JOINT_LIMIT_RANGE_HPP
+
+#include
+
+namespace base
+{
+
+struct JointLimitRange
+{
+ JointState min;
+ JointState max;
+
+ struct OutOfBounds : public std::runtime_error
+ {
+ static std::string errorString( std::string name, double min, double max, double value );
+
+ std::string name;
+ double min, max, value;
+ OutOfBounds()
+ : std::runtime_error( std::string() ), min(0), max(0), value(0) {}
+ OutOfBounds( std::string name, double min, double max, double value )
+ : std::runtime_error( errorString( name, min, max, value ) )
+ , name( name )
+ , min( min ), max( max ), value( value )
+ {
+ }
+ ~OutOfBounds() throw() {}
+ };
+
+ /**
+ * Checks that the provided JointState is within limits for the values that are valid
+ *
+ * @throw OutOfBoundsException if limits are exceeded.
+ */
+ void validate( const JointState &state ) const;
+
+ /**
+ * Checks that the provided JointState is within limits for the values that are valid
+ *
+ * @return true if the check passes, false otherwise
+ */
+ bool isValid( const JointState &state ) const;
+
+ /** Creates a JointLimitRange structure with the position range set to \c
+ * min, \c max
+ */
+ static JointLimitRange Position(double min, double max);
+
+ /** Creates a JointLimitRange structure with the speed range set to \c
+ * min, \c max
+ */
+ static JointLimitRange Speed(double min, double max);
+
+ /** Creates a JointLimitRange structure with the effort range set to \c
+ * min, \c max
+ */
+ static JointLimitRange Effort(double min, double max);
+
+ /** Creates a JointLimitRange structure with the raw range set to \c
+ * min, \c max
+ */
+ static JointLimitRange Raw(double min, double max);
+
+ /** Creates a JointLimitRange structure with the acceleration range set to \c
+ * min, \c max
+ */
+ static JointLimitRange Acceleration(double min, double max);
+
+
+private:
+ /** Internal helper method for validate and isValid */
+ std::pair verifyValidity( const JointState& state ) const;
+
+};
+
+}
+
+#endif
diff --git a/src/JointLimits.cpp b/src/JointLimits.cpp
new file mode 100644
index 00000000..67dff650
--- /dev/null
+++ b/src/JointLimits.cpp
@@ -0,0 +1,36 @@
+#include "JointLimits.hpp"
+
+namespace base {
+
+bool JointLimits::isValid(const samples::Joints& joints) const
+{
+ if (joints.hasNames())
+ {
+ for( size_t i=0; i
+#include
+
+namespace base
+{
+ struct JointLimits : public NamedVector
+ {
+ bool isValid( const base::samples::Joints& joints ) const;
+
+ /**
+ * Makes sure that all jointstate are within their respective limits
+ *
+ * Will throw if this is not the case. Will also throw if there are no
+ * limits for a particular joint.
+ */
+ void validate( const base::samples::Joints& joints ) const;
+ };
+}
+
+#endif
diff --git a/src/JointState.cpp b/src/JointState.cpp
new file mode 100644
index 00000000..0efa255b
--- /dev/null
+++ b/src/JointState.cpp
@@ -0,0 +1,141 @@
+#include "JointState.hpp"
+
+namespace base {
+
+JointState JointState::Position(double value)
+{
+ JointState ret;
+ ret.position = value;
+ return ret;
+}
+
+JointState JointState::Speed(float value)
+{
+ JointState ret;
+ ret.speed = value;
+ return ret;
+}
+
+JointState JointState::Effort(float value)
+{
+ JointState ret;
+ ret.effort = value;
+ return ret;
+}
+
+JointState JointState::Raw(float value)
+{
+ JointState ret;
+ ret.raw = value;
+ return ret;
+}
+
+JointState JointState::Acceleration(float value)
+{
+ JointState ret;
+ ret.acceleration = value;
+ return ret;
+}
+
+bool JointState::hasPosition() const
+{
+ return !isUnset(position);
+}
+
+bool JointState::hasSpeed() const
+{
+ return !isUnset(speed);
+}
+
+bool JointState::hasEffort() const
+{
+ return !isUnset(effort);
+}
+
+bool JointState::hasRaw() const
+{
+ return !isUnset(raw);
+}
+
+bool JointState::hasAcceleration() const
+{
+ return !isUnset(acceleration);
+}
+
+bool JointState::isPosition() const
+{
+ return hasPosition() && !hasSpeed() && !hasEffort() && !hasRaw() && !hasAcceleration();
+}
+
+bool JointState::isSpeed() const
+{
+ return !hasPosition() && hasSpeed() && !hasEffort() && !hasRaw() && !hasAcceleration();
+}
+
+bool JointState::isEffort() const
+{
+ return !hasPosition() && !hasSpeed() && hasEffort() && !hasRaw() && !hasAcceleration();
+}
+
+bool JointState::isRaw() const
+{
+ return !hasPosition() && !hasSpeed() && !hasEffort() && hasRaw() && !hasAcceleration();
+}
+
+bool JointState::isAcceleration() const
+{
+ return !hasPosition() && !hasSpeed() && !hasEffort() && !hasRaw() && hasAcceleration();
+}
+
+double JointState::getField(int mode) const
+{
+ switch(mode)
+ {
+ case POSITION: return position;
+ case SPEED: return speed;
+ case EFFORT: return effort;
+ case RAW: return raw;
+ case ACCELERATION: return acceleration;
+ default: throw std::runtime_error("invalid mode given to getField");
+ }
+}
+
+void JointState::setField(int mode, double value)
+{
+ switch(mode)
+ {
+ case POSITION:
+ position = value;
+ return;
+ case SPEED:
+ speed = value;
+ return;
+ case EFFORT:
+ effort = value;
+ return;
+ case RAW:
+ raw = value;
+ return;
+ case ACCELERATION:
+ acceleration = value;
+ return;
+ default: throw std::runtime_error("invalid mode given to getField");
+ }
+}
+
+JointState::MODE JointState::getMode() const
+{
+ if (isPosition()) return POSITION;
+ else if (isSpeed()) return SPEED;
+ else if (isEffort()) return EFFORT;
+ else if (isRaw()) return RAW;
+ else if (isAcceleration()) return ACCELERATION;
+ else if (hasPosition() || hasSpeed() || hasEffort() || hasRaw() || hasAcceleration())
+ throw std::runtime_error("getMode() called on a JointState that has more than one field set");
+ else
+ return UNSET;
+}
+
+
+} //end namespace base
+
diff --git a/base/JointState.hpp b/src/JointState.hpp
similarity index 61%
rename from base/JointState.hpp
rename to src/JointState.hpp
index d18e0dfd..8a5759c3 100644
--- a/base/JointState.hpp
+++ b/src/JointState.hpp
@@ -69,63 +69,38 @@ namespace base
/** Returns a JointState object with the position field set to the given
* value
*/
- static JointState Position(double value)
- {
- JointState ret;
- ret.position = value;
- return ret;
- }
+ static JointState Position(double value);
/** Returns a JointState object with the speed field set to the given
* value
*/
- static JointState Speed(float value)
- {
- JointState ret;
- ret.speed = value;
- return ret;
- }
+ static JointState Speed(float value);
/** Returns a JointState object with the effort field set to the given
* value
*/
- static JointState Effort(float value)
- {
- JointState ret;
- ret.effort = value;
- return ret;
- }
+ static JointState Effort(float value);
/** Returns a JointState object with the raw field set to the given
* value
*/
- static JointState Raw(float value)
- {
- JointState ret;
- ret.raw = value;
- return ret;
- }
+ static JointState Raw(float value);
/** Returns a JointState object with the acceleration field set to the given
* value
*/
- static JointState Acceleration(float value)
- {
- JointState ret;
- ret.acceleration = value;
- return ret;
- }
+ static JointState Acceleration(float value);
/** Tests whether the position field is set */
- bool hasPosition() const { return !base::isUnset(position); }
+ bool hasPosition() const;
/** Tests whether the speed field is set */
- bool hasSpeed() const { return !base::isUnset(speed); }
+ bool hasSpeed() const;
/** Tests whether the effort field is set */
- bool hasEffort() const { return !base::isUnset(effort); }
+ bool hasEffort() const;
/** Tests whether the raw field is set */
- bool hasRaw() const { return !base::isUnset(raw); }
+ bool hasRaw() const;
/** Tests whether the acceleration field is set */
- bool hasAcceleration() const { return !base::isUnset(acceleration); }
+ bool hasAcceleration() const;
/** Tests whether the position field is the only field set
*
@@ -133,21 +108,21 @@ namespace base
* structure is a valid command for said controller (e.g. a position
* controller would throw if isPosition() returns false)
*/
- bool isPosition() const { return hasPosition() && !hasSpeed() && !hasEffort() && !hasRaw() && !hasAcceleration(); }
+ bool isPosition() const;
/** Tests whether the speed field is the only field set
*
* This is commonly used in controllers to test whether this data
* structure is a valid command for said controller (e.g. a speed
* controller would throw if isSpeed() returns false)
*/
- bool isSpeed() const { return !hasPosition() && hasSpeed() && !hasEffort() && !hasRaw() && !hasAcceleration(); }
+ bool isSpeed() const;
/** Tests whether the effort field is the only field set
*
* This is commonly used in controllers to test whether this data
* structure is a valid command for said controller (e.g. a torque
* controller would throw if isEffort() returns false)
*/
- bool isEffort() const { return !hasPosition() && !hasSpeed() && hasEffort() && !hasRaw() && !hasAcceleration(); }
+ bool isEffort() const;
/** Tests whether the raw field is the only field set
*
* This is commonly used in controllers to test whether this data
@@ -155,7 +130,7 @@ namespace base
* driver that is controlled in PWM would throw if isRaw() returns
* false)
*/
- bool isRaw() const { return !hasPosition() && !hasSpeed() && !hasEffort() && hasRaw() && !hasAcceleration(); }
+ bool isRaw() const;
/** Tests whether the acceleration field is the only field set
*
* This is commonly used in controllers to test whether this data
@@ -163,51 +138,19 @@ namespace base
* driver that is controlled in acceleration would throw if isAcceleration() returns
* false)
*/
- bool isAcceleration() const { return !hasPosition() && !hasSpeed() && !hasEffort() && !hasRaw() && hasAcceleration(); }
+ bool isAcceleration() const;
/** Returns the field that matches the given mode
*
* @see MODE
*/
- double getField(int mode) const
- {
- switch(mode)
- {
- case POSITION: return position;
- case SPEED: return speed;
- case EFFORT: return effort;
- case RAW: return raw;
- case ACCELERATION: return acceleration;
- default: throw std::runtime_error("invalid mode given to getField");
- }
- }
+ double getField(int mode) const;
/** Sets the given field to the given value
*
* @see MODE
*/
- void setField(int mode, double value)
- {
- switch(mode)
- {
- case POSITION:
- position = value;
- return;
- case SPEED:
- speed = value;
- return;
- case EFFORT:
- effort = value;
- return;
- case RAW:
- raw = value;
- return;
- case ACCELERATION:
- acceleration = value;
- return;
- default: throw std::runtime_error("invalid mode given to getField");
- }
- }
+ void setField(int mode, double value);
/** Returns the mode of this JointState
*
@@ -217,18 +160,7 @@ namespace base
*
* @see MODE
*/
- MODE getMode() const
- {
- if (isPosition()) return POSITION;
- else if (isSpeed()) return SPEED;
- else if (isEffort()) return EFFORT;
- else if (isRaw()) return RAW;
- else if (isAcceleration()) return ACCELERATION;
- else if (hasPosition() || hasSpeed() || hasEffort() || hasRaw() || hasAcceleration())
- throw std::runtime_error("getMode() called on a JointState that has more than one field set");
- else
- return UNSET;
- }
+ MODE getMode() const;
};
}
diff --git a/src/JointTransform.cpp b/src/JointTransform.cpp
new file mode 100644
index 00000000..308144b5
--- /dev/null
+++ b/src/JointTransform.cpp
@@ -0,0 +1,32 @@
+#include "JointTransform.hpp"
+
+namespace base {
+
+void JointTransformVector::setRigidBodyStates(const samples::Joints& joints, std::vector< samples::RigidBodyState >& rbs) const
+{
+ if (joints.names.empty()) {
+ throw std::runtime_error("JointTransformVector::"
+ "setRigidBodyStates(): the vector "
+ "'joints.names()' is empty");
+ }
+
+ rbs.resize( joints.size() );
+ for( size_t i=0; i
+#include
+#include
+#include
+#include
+
+namespace base
+{
+
+/**
+ * defines the frame transformation for a joint,
+ * and the rotation axis
+ */
+struct JointTransform
+{
+ std::string sourceFrame;
+ std::string targetFrame;
+ base::Vector3d rotationAxis;
+};
+
+/**
+ * Vector of JointTranformations with added functionality
+ * to fill a vector of RigidBodyStates given a jointsState sample
+ */
+struct JointTransformVector : public base::NamedVector
+{
+ /**
+ * will fill the rbs structure with the information from the joints and
+ * the JointTransform configuration.
+ */
+ void setRigidBodyStates( const base::samples::Joints& joints, std::vector& rbs ) const;
+};
+
+}
+
+#endif
diff --git a/src/JointsTrajectory.cpp b/src/JointsTrajectory.cpp
new file mode 100644
index 00000000..863c34ef
--- /dev/null
+++ b/src/JointsTrajectory.cpp
@@ -0,0 +1,80 @@
+#include "JointsTrajectory.hpp"
+
+namespace base {
+
+bool JointsTrajectory::isValid() const
+{
+ size_t samples = getTimeSteps();
+
+ for(size_t i=0; iresize(num_joints);
+ for(size_t i=0; i getTimeSteps())
+ throw(InvalidTimeStep(time_step));
+
+ joints.resize(getNumberOfJoints());
+ joints.names = names;
+ for(size_t i=0; iresize(num_joints);
- for(size_t i=0; i getTimeSteps())
- throw(InvalidTimeStep(time_step));
-
- joints.resize(getNumberOfJoints());
- joints.names = names;
- for(size_t i=0; i
+
+namespace base {
+
+/**
+* Guarantee Symmetric (semi-) Positive Definite (SPD) matrix.
+* The input matrix must be symmetric already (only the lower triangular part will be considered)
+*/
+template
+static typename _MatrixType::PlainObject guaranteeSPD (const _MatrixType &A, const typename _MatrixType::RealScalar& minEig = 0.0)
+{
+ typedef typename _MatrixType::PlainObject Matrix;
+ typedef Eigen::SelfAdjointEigenSolver EigenDecomp;
+ typedef typename EigenDecomp::RealVectorType EigenValues;
+
+ // Eigenvalue decomposition
+ Eigen::SelfAdjointEigenSolver eigOfA (A, Eigen::ComputeEigenvectors);
+
+ // truncate Eigenvalues:
+ EigenValues s = eigOfA.eigenvalues();
+ s = s.cwiseMax(minEig);
+
+ // re-compose matrix:
+ Matrix spdA = eigOfA.eigenvectors() * s.asDiagonal() * eigOfA.eigenvectors().adjoint();
+
+ return spdA;
+};
+
+
+
+} // end namespace base
\ No newline at end of file
diff --git a/base/NamedVector.hpp b/src/NamedVector.hpp
similarity index 100%
rename from base/NamedVector.hpp
rename to src/NamedVector.hpp
diff --git a/base/Point.hpp b/src/Point.hpp
similarity index 100%
rename from base/Point.hpp
rename to src/Point.hpp
diff --git a/src/Pose.cpp b/src/Pose.cpp
new file mode 100644
index 00000000..dadfdfd2
--- /dev/null
+++ b/src/Pose.cpp
@@ -0,0 +1,165 @@
+#include "Pose.hpp"
+
+namespace base
+{
+
+Vector3d getEuler(const Orientation& orientation)
+{
+ const Eigen::Matrix3d m = orientation.toRotationMatrix();
+ double x = Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm();
+ Vector3d res(0,::atan2(-m.coeff(2,0), x),0);
+ if (x > Eigen::NumTraits::dummy_precision()){
+ res[0] = ::atan2(m.coeff(1,0), m.coeff(0,0));
+ res[2] = ::atan2(m.coeff(2,1), m.coeff(2,2));
+ }else{
+ res[0] = 0;
+ res[2] = (m.coeff(2,0)>0?1:-1)* ::atan2(-m.coeff(0,1), m.coeff(1,1));
+ }
+ return res;
+}
+
+Vector3d getEuler(const AngleAxisd& orientation)
+{
+ const Eigen::Matrix3d m = orientation.toRotationMatrix();
+ double x = Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm();
+ Vector3d res(0,::atan2(-m.coeff(2,0), x),0);
+ if (x > Eigen::NumTraits::dummy_precision()){
+ res[0] = ::atan2(m.coeff(1,0), m.coeff(0,0));
+ res[2] = ::atan2(m.coeff(2,1), m.coeff(2,2));
+ }else{
+ res[0] = 0;
+ res[2] = (m.coeff(2,0)>0?1:-1)* ::atan2(-m.coeff(0,1), m.coeff(1,1));
+ }
+ return res;
+}
+
+double getYaw(const Orientation& orientation)
+{
+ return getEuler(orientation)[0];
+}
+
+double getYaw(const AngleAxisd& orientation)
+{
+ return getEuler(orientation)[0];
+}
+
+double getPitch(const Orientation& orientation)
+{
+ return getEuler(orientation)[1];
+}
+
+double getPitch(const AngleAxisd& orientation)
+{
+ return getEuler(orientation)[1];
+}
+
+double getRoll(const Orientation& orientation)
+{
+ return getEuler(orientation)[2];
+}
+
+double getRoll(const AngleAxisd& orientation)
+{
+ return getEuler(orientation)[2];
+}
+
+Orientation removeYaw(const Orientation& orientation)
+{
+ return Eigen::AngleAxisd( -getYaw(orientation), Eigen::Vector3d::UnitZ()) * orientation;
+}
+
+Orientation removeYaw(const AngleAxisd& orientation)
+{
+ return Eigen::AngleAxisd( -getYaw(orientation), Eigen::Vector3d::UnitZ()) * orientation;
+}
+
+Orientation removePitch(const Orientation& orientation)
+{
+ return Eigen::AngleAxisd( -getPitch(orientation), Eigen::Vector3d::UnitY()) * orientation;
+}
+
+Orientation removePitch(const AngleAxisd& orientation)
+{
+ return Eigen::AngleAxisd( -getPitch(orientation), Eigen::Vector3d::UnitY()) * orientation;
+}
+
+Orientation removeRoll(const Orientation& orientation)
+{
+ return Eigen::AngleAxisd( -getRoll(orientation), Eigen::Vector3d::UnitX()) * orientation;
+}
+
+Orientation removeRoll(const AngleAxisd& orientation)
+{
+ return Eigen::AngleAxisd( -getRoll(orientation), Eigen::Vector3d::UnitX()) * orientation;
+}
+
+
+PoseUpdateThreshold::PoseUpdateThreshold()
+{
+}
+
+PoseUpdateThreshold::PoseUpdateThreshold(double _distance, double _angle): distance( _distance ), angle( _angle )
+{
+}
+
+bool PoseUpdateThreshold::test( double other_distance, double other_angle )
+{
+ return other_distance > distance || other_angle > angle;
+}
+
+bool PoseUpdateThreshold::test( const Eigen::Affine3d& pdelta )
+{
+ return test( pdelta.translation().norm(), Eigen::AngleAxisd( pdelta.linear() ).angle() );
+}
+
+bool PoseUpdateThreshold::test(const Eigen::Affine3d& a2b, const Eigen::Affine3d& aprime2b)
+{
+ return test( a2b.inverse() * aprime2b );
+}
+
+
+void Pose::fromVector6d(const Vector6d& v)
+{
+ const Eigen::Vector3d saxis = v.head<3>();
+ if( saxis.norm() > 1e-9 )
+ orientation = Eigen::AngleAxisd( saxis.norm(), saxis.normalized() );
+ else
+ orientation = Eigen::Quaterniond::Identity();
+
+ position = v.tail<3>();
+}
+
+Vector6d Pose::toVector6d() const
+{
+ Vector6d res;
+ Eigen::AngleAxisd aa(orientation);
+ res.head<3>() = aa.axis() * aa.angle();
+ res.tail<3>() = position;
+
+ return res;
+}
+
+std::ostream& operator << (std::ostream& io, Pose const& pose)
+{
+ io << "Position "
+ << pose.position.transpose()
+ << " Orientation (RPY)"
+ << getRoll(pose.orientation) << " "
+ << getPitch(pose.orientation) << " "
+ << getYaw(pose.orientation);
+ return io;
+}
+
+std::ostream& operator << (std::ostream& io, Pose2D const& pose)
+{
+
+ io << "Position "
+ << pose.position.transpose()
+ << " Orientation (Theta) "
+ << pose.orientation ;
+ return io;
+}
+
+
+
+} //end namespace base
\ No newline at end of file
diff --git a/base/Pose.hpp b/src/Pose.hpp
similarity index 52%
rename from base/Pose.hpp
rename to src/Pose.hpp
index 0551e357..08667e87 100644
--- a/base/Pose.hpp
+++ b/src/Pose.hpp
@@ -1,7 +1,7 @@
#ifndef __BASE_POSE_HH__
#define __BASE_POSE_HH__
-#include
+#include "Eigen.hpp"
#include "Angle.hpp"
namespace base
@@ -25,19 +25,7 @@ namespace base
* assuming angles in range of: a0:(-pi,pi), a1:(-pi/2,pi/2), a2:(-pi/2,pi/2)
*
*/
- static base::Vector3d getEuler(const base::Orientation &orientation){
- const Eigen::Matrix3d m = orientation.toRotationMatrix();
- double x = base::Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm();
- base::Vector3d res(0,::atan2(-m.coeff(2,0), x),0);
- if (x > Eigen::NumTraits::dummy_precision()){
- res[0] = ::atan2(m.coeff(1,0), m.coeff(0,0));
- res[2] = ::atan2(m.coeff(2,1), m.coeff(2,2));
- }else{
- res[0] = 0;
- res[2] = (m.coeff(2,0)>0?1:-1)* ::atan2(-m.coeff(0,1), m.coeff(1,1));
- }
- return res;
- }
+ base::Vector3d getEuler(const base::Orientation &orientation);
/*
* Decomposes the orientation in euler angles so that this can be
@@ -50,79 +38,32 @@ namespace base
* assuming angles in range of: a0:(-pi,pi), a1:(-pi/2,pi/2), a2:(-pi/2,pi/2)
*
*/
- static base::Vector3d getEuler(const base::AngleAxisd &orientation){
- const Eigen::Matrix3d m = orientation.toRotationMatrix();
- double x = base::Vector2d(m.coeff(2,2) , m.coeff(2,1)).norm();
- base::Vector3d res(0,::atan2(-m.coeff(2,0), x),0);
- if (x > Eigen::NumTraits::dummy_precision()){
- res[0] = ::atan2(m.coeff(1,0), m.coeff(0,0));
- res[2] = ::atan2(m.coeff(2,1), m.coeff(2,2));
- }else{
- res[0] = 0;
- res[2] = (m.coeff(2,0)>0?1:-1)* ::atan2(-m.coeff(0,1), m.coeff(1,1));
- }
- return res;
- }
+ base::Vector3d getEuler(const base::AngleAxisd &orientation);
- static double getYaw(const base::Orientation& orientation)
- {
- return base::getEuler(orientation)[0];
- }
+ double getYaw(const base::Orientation& orientation);
+
- static double getYaw(const base::AngleAxisd& orientation)
- {
- return base::getEuler(orientation)[0];
- }
+ double getYaw(const base::AngleAxisd& orientation);
- static double getPitch(const base::Orientation& orientation)
- {
- return base::getEuler(orientation)[1];
- }
+ double getPitch(const base::Orientation& orientation);
- static double getPitch(const base::AngleAxisd& orientation)
- {
- return base::getEuler(orientation)[1];
- }
+ double getPitch(const base::AngleAxisd& orientation);
- static double getRoll(const base::Orientation& orientation)
- {
- return base::getEuler(orientation)[2];
- }
+ double getRoll(const base::Orientation& orientation);
- static double getRoll(const base::AngleAxisd& orientation)
- {
- return base::getEuler(orientation)[2];
- }
+ double getRoll(const base::AngleAxisd& orientation);
- static inline base::Orientation removeYaw(const base::Orientation& orientation)
- {
- return Eigen::AngleAxisd( -getYaw(orientation), Eigen::Vector3d::UnitZ()) * orientation;
- }
+ base::Orientation removeYaw(const base::Orientation& orientation);
- static inline base::Orientation removeYaw(const base::AngleAxisd& orientation)
- {
- return Eigen::AngleAxisd( -getYaw(orientation), Eigen::Vector3d::UnitZ()) * orientation;
- }
+ base::Orientation removeYaw(const base::AngleAxisd& orientation);
- static inline base::Orientation removePitch(const base::Orientation& orientation)
- {
- return Eigen::AngleAxisd( -getPitch(orientation), Eigen::Vector3d::UnitY()) * orientation;
- }
+ base::Orientation removePitch(const base::Orientation& orientation);
- static inline base::Orientation removePitch(const base::AngleAxisd& orientation)
- {
- return Eigen::AngleAxisd( -getPitch(orientation), Eigen::Vector3d::UnitY()) * orientation;
- }
+ base::Orientation removePitch(const base::AngleAxisd& orientation);
- static inline base::Orientation removeRoll(const base::Orientation& orientation)
- {
- return Eigen::AngleAxisd( -getRoll(orientation), Eigen::Vector3d::UnitX()) * orientation;
- }
+ base::Orientation removeRoll(const base::Orientation& orientation);
- static inline base::Orientation removeRoll(const base::AngleAxisd& orientation)
- {
- return Eigen::AngleAxisd( -getRoll(orientation), Eigen::Vector3d::UnitX()) * orientation;
- }
+ base::Orientation removeRoll(const base::AngleAxisd& orientation);
/**
* Represents a pose update threshold, with a number of test methods to see
@@ -130,31 +71,24 @@ namespace base
*/
struct PoseUpdateThreshold
{
- PoseUpdateThreshold() {};
+ PoseUpdateThreshold();
/**
* Constructor with distance and angle thresholds
*/
- PoseUpdateThreshold( double _distance, double _angle )
- : distance( _distance ), angle( _angle ) {};
+ PoseUpdateThreshold( double _distance, double _angle );
/**
* Test if distance or angle is greater than the
* stored threshold.
*/
- bool test( double other_distance, double other_angle )
- {
- return other_distance > distance || other_angle > angle;
- }
+ bool test( double other_distance, double other_angle );
/**
* Test if the provided delta transformation is greater in
* either distance or angle than the threshold
*/
- bool test( const Eigen::Affine3d& pdelta )
- {
- return test( pdelta.translation().norm(), Eigen::AngleAxisd( pdelta.linear() ).angle() );
- }
+ bool test( const Eigen::Affine3d& pdelta );
/**
* Test if the delta of the provided transformations is greater in
@@ -165,10 +99,7 @@ namespace base
*
* @result true if the transformation A' to A is greater than the stored thresholds
*/
- bool test( const Eigen::Affine3d& a2b, const Eigen::Affine3d& aprime2b )
- {
- return test( a2b.inverse() * aprime2b );
- }
+ bool test( const Eigen::Affine3d& a2b, const Eigen::Affine3d& aprime2b );
double distance;
double angle;
@@ -243,16 +174,7 @@ namespace base
* @param v compact 6 vector [r t], where r is a 3 vector representing
* the rotation in scaled axis form, and t is the translation 3 vector.
*/
- void fromVector6d( const Vector6d &v )
- {
- const Eigen::Vector3d saxis = v.head<3>();
- if( saxis.norm() > 1e-9 )
- orientation = Eigen::AngleAxisd( saxis.norm(), saxis.normalized() );
- else
- orientation = Eigen::Quaterniond::Identity();
-
- position = v.tail<3>();
- }
+ void fromVector6d( const Vector6d &v );
/**
* @brief get compact scaled-axis representation of pose
@@ -260,15 +182,7 @@ namespace base
* @result compact 6 vector [r t], where r is a 3 vector representing
* the rotation in scaled axis form, and t is the translation 3 vector.
*/
- Vector6d toVector6d() const
- {
- Vector6d res;
- Eigen::AngleAxisd aa(orientation);
- res.head<3>() = aa.axis() * aa.angle();
- res.tail<3>() = position;
-
- return res;
- }
+ Vector6d toVector6d() const;
/**
* @result yaw (rotation around z-axis) part of the rotational part of the pose
@@ -279,17 +193,7 @@ namespace base
}
};
- inline std::ostream& operator << (std::ostream& io, base::Pose const& pose)
- {
- io << "Position "
- << pose.position.transpose()
- << " Orientation (RPY)"
- << getRoll(pose.orientation) << " "
- << getPitch(pose.orientation) << " "
- << getYaw(pose.orientation);
-;
- return io;
- }
+ std::ostream& operator << (std::ostream& io, base::Pose const& pose);
/**
* Representation for a pose in 2D
@@ -319,16 +223,7 @@ namespace base
};
- inline std::ostream& operator << (std::ostream& io, base::Pose2D const& pose)
- {
-
- io << "Position "
- << pose.position.transpose()
- << " Orientation (Theta) "
- << pose.orientation ;
-;
- return io;
- }
+ std::ostream& operator << (std::ostream& io, base::Pose2D const& pose);
}
#endif
diff --git a/src/Pressure.cpp b/src/Pressure.cpp
new file mode 100644
index 00000000..2c0b8df6
--- /dev/null
+++ b/src/Pressure.cpp
@@ -0,0 +1,48 @@
+#include "Pressure.hpp"
+
+#include "Float.hpp"
+
+namespace base {
+
+Pressure::Pressure() : pascal(unknown())
+{
+
+}
+
+Pressure Pressure::fromPascal(float pascal)
+{
+ Pressure result;
+ result.pascal = pascal;
+ return result;
+}
+
+Pressure Pressure::fromBar(float bar)
+{
+ return fromPascal(bar * 100000);
+}
+
+Pressure Pressure::fromPSI(float psi)
+{
+ return fromPascal(psi * 6894.75729);
+}
+
+float Pressure::toPa() const
+{
+ return pascal;
+}
+
+float Pressure::toBar() const
+{
+ return pascal / 100000;
+}
+
+float Pressure::toPSI() const
+{
+ return pascal / 6894.75729;
+}
+
+} //end namespace base
+
+
+
+
diff --git a/base/Pressure.hpp b/src/Pressure.hpp
similarity index 61%
rename from base/Pressure.hpp
rename to src/Pressure.hpp
index 4fdcd8cf..95762188 100644
--- a/base/Pressure.hpp
+++ b/src/Pressure.hpp
@@ -1,8 +1,6 @@
#ifndef __BASE_PRESSURE_HPP__
#define __BASE_PRESSURE_HPP__
-#include
-
namespace base
{
/** Representation of a pressure value
@@ -25,46 +23,26 @@ namespace base
/**
* default constructor, which will initialize the value to unknown (NaN)
*/
- Pressure() : pascal(base::unknown()) {}
+ Pressure();
public:
/** Create a pressure object using a value in pascals */
- static Pressure fromPascal(float pascal)
- {
- Pressure result;
- result.pascal = pascal;
- return result;
- }
+ static Pressure fromPascal(float pascal);
/** Create a pressure object using a value in bars */
- static Pressure fromBar(float bar)
- {
- return fromPascal(bar * 100000);
- }
+ static Pressure fromBar(float bar);
/** Create a pressure object using a value in PSI */
- static Pressure fromPSI(float psi)
- {
- return fromPascal(psi * 6894.75729);
- }
+ static Pressure fromPSI(float psi);
/** Returns the raw pressure value in pascals */
- float toPa() const
- {
- return pascal;
- }
+ float toPa() const;
/** Returns the raw pressure value in bar */
- float toBar() const
- {
- return pascal / 100000;
- }
+ float toBar() const;
/** Returns the raw pressure value in psi */
- float toPSI() const
- {
- return pascal / 6894.75729;
- }
+ float toPSI() const;
};
}
diff --git a/src/Spline.cpp b/src/Spline.cpp
index 4772638c..346d7958 100644
--- a/src/Spline.cpp
+++ b/src/Spline.cpp
@@ -8,10 +8,11 @@
#include
using namespace std;
-using namespace base::geometry;
using boost::lexical_cast;
using namespace Eigen;
+
+
static double angleLimit(double angle)
{
if(angle > M_PI)
@@ -22,6 +23,8 @@ static double angleLimit(double angle)
return angle;
}
+namespace base { namespace geometry {
+
SplineBase::SplineBase (int dim, double _geometric_resolution, int _curve_order)
: dimension(dim), curve(0), geometric_resolution(_geometric_resolution)
, curve_order(_curve_order)
@@ -99,7 +102,7 @@ void SplineBase::getPoint(double* result, double _param) const
void SplineBase::getPointAndTangent(double* result, double _param) const
{ return getPointAndTangentHelper(result, _param, true); }
-bool base::geometry::SplineBase::checkAndNormalizeParam(double& _param, double equalDistance) const
+bool SplineBase::checkAndNormalizeParam(double& _param, double equalDistance) const
{
if(_param < start_param && start_param - _param < equalDistance)
_param = start_param;
@@ -184,7 +187,7 @@ double SplineBase::getVariationOfCurvature(double _param) // Variation of Curva
return VoC;
}
-double base::geometry::SplineBase::getCurveLength(double relative_resolution) const
+double SplineBase::getCurveLength(double relative_resolution) const
{
if (isSingleton())
return 0;
@@ -288,7 +291,7 @@ void SplineBase::interpolate(const vector< double >& points,
else
{
if (coord_types.size() * dimension != points.size()) {
- throw std::runtime_error("base::types::Spline::interpolate(): "
+ throw std::runtime_error("types::Spline::interpolate(): "
"'points.size()' does not match "
"expectation");
}
@@ -852,7 +855,7 @@ double SplineBase::join(SplineBase const& other, double tolerance, bool with_tan
//be sure joining_Types has the right size
if (joining_points.size() / dim > 4) {
- throw std::runtime_error("base::SplineBase::join(): the size of "
+ throw std::runtime_error("SplineBase::join(): the size of "
"'joining_points' is unexpectedly big");
}
@@ -969,7 +972,7 @@ SISLCurve* SplineBase::getSISLCurve()
return curve;
}
-base::Matrix3d SplineBase::getFrenetFrame(double _param)
+Matrix3d SplineBase::getFrenetFrame(double _param)
{
if (!curve)
throw std::runtime_error("getFrenetFrame() called on an empty or degenerate curve");
@@ -1006,7 +1009,7 @@ double SplineBase::headingError(double _actHeading, double _param)
return angleLimit( _actHeading - getHeading(_param));
}
-double SplineBase::distanceError(base::Vector3d _pt, double _param)
+double SplineBase::distanceError(Vector3d _pt, double _param)
{
// Error vector
Vector3d curve_point;
@@ -1024,7 +1027,7 @@ double SplineBase::distanceError(base::Vector3d _pt, double _param)
return (angle >= 0.0)?(error.norm()):(-error.norm());
}
-base::Vector3d SplineBase::poseError(base::Vector3d _position, double _heading, double _guess, double minParam)
+Vector3d SplineBase::poseError(Vector3d _position, double _heading, double _guess, double minParam)
{
double param = findOneClosestPoint(_position.data(), _guess, getGeometricResolution());
@@ -1032,15 +1035,15 @@ base::Vector3d SplineBase::poseError(base::Vector3d _position, double _heading,
param = minParam;
// Returns the error [distance error, orientation error, parameter]
- return base::Vector3d(distanceError(_position, param), headingError(_heading, param), param);
+ return Vector3d(distanceError(_position, param), headingError(_heading, param), param);
}
-base::Vector3d SplineBase::poseError(base::Vector3d _position, double _heading, double _guess)
+Vector3d SplineBase::poseError(Vector3d _position, double _heading, double _guess)
{
double param = findOneClosestPoint(_position.data(), _guess, getGeometricResolution());
// Returns the error [distance error, orientation error, parameter]
- return base::Vector3d(distanceError(_position, param), headingError(_heading, param), param);
+ return Vector3d(distanceError(_position, param), headingError(_heading, param), param);
}
void SplineBase::crop(double start_t, double end_t)
@@ -1143,3 +1146,5 @@ SplineBase *SplineBase::getSubSpline(double start_t, double end_t) const
return new SplineBase(getGeometricResolution(), newCurve);
}
+
+}} //end namespace base::geometry
diff --git a/src/Temperature.cpp b/src/Temperature.cpp
new file mode 100644
index 00000000..ac3b0b36
--- /dev/null
+++ b/src/Temperature.cpp
@@ -0,0 +1,111 @@
+#include "Temperature.hpp"
+
+#include
+#include
+
+namespace base {
+
+Temperature::Temperature() : kelvin(unknown())
+{
+
+}
+
+Temperature::Temperature(double kelvin) : kelvin(kelvin)
+{
+
+}
+
+double Temperature::kelvin2Celsius(double kelvin)
+{
+ return kelvin - 273.15;
+}
+
+double Temperature::celsius2Kelvin(double celsius)
+{
+ return celsius + 273.15;
+}
+
+Temperature Temperature::fromKelvin(double kelvin)
+{
+ return Temperature( kelvin );
+}
+
+Temperature Temperature::fromCelsius(double celsius)
+{
+ return Temperature( celsius + 273.15);
+}
+
+double Temperature::getKelvin() const
+{
+ return kelvin;
+}
+
+double Temperature::getCelsius() const
+{
+ return kelvin - 273.15;
+}
+
+bool Temperature::isApprox(Temperature other, double prec) const
+{
+ return std::abs( other.kelvin - kelvin ) < prec;
+}
+
+void Temperature::operator=(const Temperature& other)
+{
+ kelvin = other.kelvin;
+}
+
+bool Temperature::operator==(const Temperature& other) const
+{
+ return this->kelvin == other.kelvin;
+}
+
+bool Temperature::operator<(const Temperature& other) const
+{
+ return this->kelvin < other.kelvin;
+}
+
+bool Temperature::operator>(const Temperature& other) const
+{
+ return this->kelvin > other.kelvin;
+}
+
+Temperature operator+(Temperature a, Temperature b)
+{
+ return Temperature::fromKelvin( a.getKelvin() + b.getKelvin() );
+}
+
+Temperature operator-(Temperature a, Temperature b)
+{
+ return Temperature::fromKelvin( a.getKelvin() - b.getKelvin() );
+}
+
+Temperature operator*(Temperature a, double b)
+{
+ return Temperature::fromKelvin( a.getKelvin() * b );
+}
+
+Temperature operator*(double a, Temperature b)
+{
+ return Temperature::fromKelvin( a * b.getKelvin() );
+}
+
+std::ostream& operator<<(std::ostream& os, Temperature temperature)
+{
+ os << temperature.getCelsius() << boost::format("[%3.1f celsius]");
+ return os;
+}
+
+bool Temperature::isInRange(const Temperature &left_limit, const Temperature &right_limit) const
+{
+ if((right_limit-left_limit).kelvin < 0)
+ return !isInRange(right_limit,left_limit);
+ if((*this -left_limit).getKelvin() >= 0 && (right_limit -*this).getKelvin() >= 0)
+ return true;
+ return false;
+}
+
+
+} //end namespace base
+
+
diff --git a/src/Temperature.hpp b/src/Temperature.hpp
new file mode 100644
index 00000000..6d0862ca
--- /dev/null
+++ b/src/Temperature.hpp
@@ -0,0 +1,113 @@
+#ifndef __BASE_TEMPERATURE_HH__
+#define __BASE_TEMPERATURE_HH__
+
+#include // std::abs
+
+namespace base
+{
+
+/**
+ * This class represents a temperature, and can be used instead of double for
+ * convenience. The class has a canonical representation of the temperature in
+ * kelvin.
+ */
+class Temperature
+{
+public:
+
+ /**
+ * temperature in kelvins
+ *
+ *
+ * @note don't use this value directly. It's only public to allow this class
+ * to be used as an interface type.
+ */
+ double kelvin;
+
+ /**
+ * default constructor, which will leave the temperature uninitialized.
+ */
+ Temperature();
+
+protected:
+ explicit Temperature( double kelvin );
+
+
+public:
+ /**
+ * static conversion from kelvin to celsius
+ * @param kelvin temperature in kelvin
+ * @result temperature in celsius
+ */
+ static double kelvin2Celsius( double kelvin );
+
+ /**
+ * static conversion from celsius to kelvin
+ * @param celsius temperature in celsius
+ * @result temperature in kelvin
+ */
+ static double celsius2Kelvin( double celsius );
+
+
+ /**
+ * use this method to get temperature from Kelvin.
+ * @return representation of the given temperature.
+ * @param kelvin - temperature in Kelvin.
+ */
+ static Temperature fromKelvin( double kelvin );
+
+
+ /**
+ * use this method to get temperature from Celsius
+ * @return representation of the given temperature.
+ * @param celsius - temperature in celsius.
+ */
+ static Temperature fromCelsius( double celsius );
+
+ /**
+ * @return canonical value of the temperature in kelvin
+ */
+ double getKelvin() const ;
+
+ /**
+ * @return canonical value of the temperature in celsius
+ */
+ double getCelsius() const;
+
+ /**
+ * @return true if the temperature is insight the given interval
+ */
+ bool isInRange(const Temperature &left_limit,const Temperature &right_limit) const;
+
+ /**
+ * compare two temperatures for approximate equality
+ * @param other - temperature to compare
+ * @param prec - precision interval in kelvin
+ * @return true if temperature is approximately equal
+ */
+ bool isApprox( Temperature other, double prec = 1e-5 ) const;
+
+ void operator=(const Temperature &other);
+
+ bool operator==(const Temperature &other ) const;
+
+ bool operator<(const Temperature &other ) const;
+
+ bool operator>(const Temperature &other ) const;
+
+};
+
+Temperature operator+( Temperature a, Temperature b );
+
+Temperature operator-( Temperature a, Temperature b );
+
+Temperature operator*( Temperature a, double b );
+
+Temperature operator*( double a, Temperature b );
+
+std::ostream& operator << (std::ostream& os, Temperature temperature);
+
+
+}
+
+#endif
diff --git a/src/Time.cpp b/src/Time.cpp
new file mode 100644
index 00000000..91a0f3aa
--- /dev/null
+++ b/src/Time.cpp
@@ -0,0 +1,258 @@
+#include "Time.hpp"
+
+#include
+#include
+#include
+#include
+#include
+#include