cleanup headers

lwrserv/include/trajectory.h

@@ -0,0 +1,21 @@
+#ifndef _TRAJECTORY_H_
+#define _TRAJECTORY_H_
+
+#include "vec.h"
+
+class Trajectory
+{
+    public:
+        virtual unsigned int getSteps();
+
+        virtual Vec<T, SIZE> getJointValues();
+        virtual Vec<T, SIZE> getJointVelocity(unsigned int jointID);
+        virtual Vec<T, SIZE> getJointAcceleration(unsigned int jointID);
+
+        virtual Vec<T, SIZE> getCarthesianValues();
+        virtual Vec<T, SIZE> getCarthesianVelocity(unsigned int jointID);
+        virtual Vec<T, SIZE> getCarthesianAcceleration(unsigned int jointID);
+};
+
+class TrajectoryBangBang :: Trajectory;
+#endif 

lwrserv/include/vec.h

@@ -0,0 +1,236 @@
+#ifndef _VEC_H_
+#define _VEC_H_
+#include <math.h>
+#include <ostream>
+#include "mat.h"
+
+template<class T, unsigned SIZE> class Vec;
+template <class T, unsigned SIZE> class Mat;
+
+
+// Vector class for SIMPLE data types
+template <class T, unsigned SIZE> class Vec 
+{
+public:
+
+    // standard constructor
+    Vec<T,SIZE> ()  
+    {   // initialize all elements with zero
+        for (unsigned int i=0; i<SIZE; i++)
+            m_atData[i] = T(0);
+    }
+
+    // construction with data array
+    Vec<T, SIZE> (const T atData[SIZE]) 
+    {
+        for (unsigned int i=0; i<SIZE; i++)
+            m_atData[i] = atData[i];
+    }
+
+    // copy constructor
+    Vec<T, SIZE> (const Vec<T, SIZE> &vec) 
+    {
+        if (this==&vec)
+            return; // nothing to do, it's me
+        for (unsigned int i=0; i<SIZE; i++)
+            m_atData[i] = vec.m_atData[i];
+    }
+
+    // destructor
+    ~Vec () 
+    {   // nothing to do here ...
+    }
+
+    void setData (const T atData[SIZE]) 
+    {
+        for (unsigned int i=0; i<SIZE; i++)
+            m_atData[i] = atData[i];
+    }
+
+    void getData (const T atData[SIZE]) 
+    {
+        for (unsigned int i=0; i<SIZE; i++)
+            atData[i] = m_atData[i];
+        
+    }
+
+    unsigned getDimension () 
+    {
+        return SIZE;
+    }
+
+
+    Vec<T, SIZE> &operator = (const Vec<T, SIZE> &vec) 
+    {
+        if (this==&vec)
+            return (*this); // ok, it's me, so no L-value action
+        for (unsigned int i=0; i<SIZE; i++)     // not me, so L-Value action: copy data
+            m_atData[i] = vec.m_atData[i];
+
+        return (*this); // also an R-value in e.g
+                        // vec1    =       vec2      = vec3;
+                        // L-Value = R-Value/L-Value = R-Value
+    }
+
+    Vec<T, SIZE> &operator = (const T atData[SIZE]) 
+    {
+        for (unsigned int i=0; i<SIZE; i++) // not me, so L-Value action: copy data
+            m_atData[i] = atData[i];
+
+        return (*this); // also an R-value in e.g.
+                        // vec1  = vec2  +   (vec2=atData); // parenthesis () needed to evaluate expression vec2=atData
+                        // L-Val = R-Val +       R-Val
+                        //   "   =   "   + (L-Val = R-Val)
+    }
+
+    // usage of operator:
+    // vec(i) = var;    // 0 <= i <= SIZE-1
+    // var = vec(i);
+    // vec1(i) = vec2(j);
+    T &operator () (unsigned i) 
+    {
+        if (i>=SIZE)
+            i = SIZE-1; // !!! operator clips index ...
+        return m_atData[i];         // ... so we can safely return a reference
+    }
+
+    T operator () (unsigned i) const 
+    {
+        if (i>=SIZE)
+            i = SIZE-1;
+        return m_atData[i];
+    }
+
+    void operator += (const Vec<T, SIZE> &vec) 
+    {
+        for (int i=0; i<SIZE; i++)
+            m_atData[i] += vec.m_atData[i];
+    }
+
+    Vec<T, SIZE> operator + (const Vec<T, SIZE> &vec) 
+    {
+        Vec<T, SIZE> buf (m_atData);
+        for (unsigned int i=0; i<SIZE; i++)
+            buf.m_atData[i] += vec.m_atData[i];
+        return buf;
+    }
+
+    void operator -= (const Vec<T, SIZE> &vec) 
+    {
+        for (unsigned int i=0; i<SIZE; i++)
+            m_atData[i] -= vec.m_atData[i];
+    }
+
+    // binary -
+    // vec1 - vec2 i.e. (*this) - vec
+    Vec<T, SIZE> operator - (const Vec<T, SIZE> &vec) 
+    {
+        Vec<T, SIZE> buf (m_atData);
+        for (unsigned int i=0; i<SIZE; i++)
+            buf.m_atData[i] -= vec.m_atData[i];
+        return buf;
+    }
+
+    // unary -
+    // -vec i.e. -(*this)
+    Vec<T, SIZE> operator - () 
+    {
+        T atBuffer[SIZE];
+        for (int i=0; i<SIZE; i++)
+            atBuffer[i] = -m_atData[i];
+        return Vec<T, SIZE> (atBuffer);
+    }
+
+    T operator * (const Vec<T, SIZE> & vec) 
+    {
+        T dp = T(0);
+        for (int i=0; i<SIZE; i++)
+            dp += m_atData[i]*vec.m_atData[i];
+        return dp;
+    }
+
+    void operator *= (T tScale) 
+    {
+        for (unsigned int i=0; i<SIZE; i++)
+            m_atData[i] *= tScale;
+    }
+
+    Vec<T, SIZE> operator * (T tScale) 
+    {
+        T atBuffer[SIZE];
+        for (unsigned int i=0; i<SIZE; i++)
+            atBuffer[i] = m_atData[i]*tScale;
+        return Vec<T, SIZE> (atBuffer);
+    }
+
+    Vec<T, SIZE> operator * (const Mat<T, SIZE> &mat)
+    {
+        Vec<T, SIZE> vec;
+        for (unsigned int j=0; j<SIZE; j++)
+            for (unsigned int i=0; i<SIZE; i++)
+               vec(j) += m_atData[i]*mat(i,j);
+        return vec;
+    }
+
+    T norm ()
+    {
+        T retval = 0;
+
+        // this * this
+        for (int i=0; i<SIZE; i++)
+            retval += m_atData[i] * m_atData[i];
+        sqrt(retval);
+        return retval;
+    }
+
+    Vec<T, SIZE> x(const Vec<T, SIZE> &vec)
+    {
+        T temp[SIZE];
+        temp[0] = m_atData[1]*vec(2) - m_atData[2]*vec(1);
+        temp[1] = m_atData[2]*vec(0) - m_atData[0]*vec(2);
+        temp[2] = m_atData[0]*vec(1) - m_atData[1]*vec(0);
+        temp[3] = vec(3);
+        return Vec<T, SIZE> (temp);
+    }
+
+private:
+
+    T m_atData[SIZE];
+
+}; // class Vec
+
+
+// some common vector classes (abbr. names)
+typedef Vec<float, 2> Vec2f;
+typedef Vec<float, 3> Vec3f;
+typedef Vec<float, 4> Vec4f;
+
+typedef Vec<double, 2> Vec2d;
+typedef Vec<double, 3> Vec3d;
+typedef Vec<double, 4> Vec4d;
+
+typedef Vec<int, 2> Vec2i;
+typedef Vec<int, 3> Vec3i;
+typedef Vec<int, 4> Vec4i;
+
+typedef Vec<bool, 2> Vec2b;
+typedef Vec<bool, 3> Vec3b;
+typedef Vec<bool, 4> Vec4b;
+
+
+template <class T, unsigned SIZE>
+static std::ostream& operator<< (std::ostream& output,const Vec<T,SIZE> &vec)
+{
+    output << "( ";
+    for(unsigned int i = 0 ; i< SIZE; ++i)
+    {
+        output << vec(i);
+        if (i<SIZE-1)
+            output << " , ";
+        else
+            output << " )";
+    }
+    return output;
+}
+
+#endif