00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #ifndef KDL_JNTARRAYACC_HPP
00023 #define KDL_JNTARRAYACC_HPP
00024
00025 #include "utilities/utility.h"
00026 #include "jntarray.hpp"
00027 #include "jntarrayvel.hpp"
00028 #include "frameacc.hpp"
00029
00030 namespace KDL
00031 {
00032
00033 class JntArrayAcc;
00034 bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps=epsilon);
00035 void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest);
00036 void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest);
00037 void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest);
00038 void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest);
00039 void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest);
00040 void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest);
00041 void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest);
00042 void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest);
00043 void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest);
00044 void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest);
00045 void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest);
00046 void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest);
00047 void SetToZero(JntArrayAcc& array);
00048
00049 class JntArrayAcc
00050 {
00051 public:
00052 JntArray q;
00053 JntArray qdot;
00054 JntArray qdotdot;
00055 public:
00056 JntArrayAcc(){};
00057 explicit JntArrayAcc(unsigned int size);
00058 JntArrayAcc(const JntArray& q,const JntArray& qdot,const JntArray& qdotdot);
00059 JntArrayAcc(const JntArray& q,const JntArray& qdot);
00060 explicit JntArrayAcc(const JntArray& q);
00061
00062 void resize(unsigned int newSize);
00063
00064 JntArray value()const;
00065 JntArray deriv()const;
00066 JntArray dderiv()const;
00067
00068 friend void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest);
00069 friend void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest);
00070 friend void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest);
00071 friend void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest);
00072 friend void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest);
00073 friend void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest);
00074 friend void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest);
00075 friend void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest);
00076 friend void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest);
00077 friend void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest);
00078 friend void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest);
00079 friend void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest);
00080 friend void SetToZero(JntArrayAcc& array);
00081 friend bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps);
00082
00083 };
00084
00085 }
00086
00087 #endif