Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "jntarray.hpp"
00023
00024 namespace KDL
00025 {
00026 using namespace Eigen;
00027
00028 JntArray::JntArray()
00029 {
00030 }
00031
00032 JntArray::JntArray(unsigned int _size):
00033 data(_size)
00034 {
00035 data.setZero();
00036 }
00037
00038
00039 JntArray::JntArray(const JntArray& arg):
00040 data(arg.data)
00041 {
00042 }
00043
00044 JntArray& JntArray::operator = (const JntArray& arg)
00045 {
00046 data=arg.data;
00047 return *this;
00048 }
00049
00050
00051 JntArray::~JntArray()
00052 {
00053 }
00054
00055 void JntArray::resize(unsigned int newSize)
00056 {
00057 data.resize(newSize);
00058 }
00059
00060 double JntArray::operator()(unsigned int i,unsigned int j)const
00061 {
00062 assert(j==0);
00063 return data(i);
00064 }
00065
00066 double& JntArray::operator()(unsigned int i,unsigned int j)
00067 {
00068 assert(j==0);
00069 return data(i);
00070 }
00071
00072 unsigned int JntArray::rows()const
00073 {
00074 return data.rows();
00075 }
00076
00077 unsigned int JntArray::columns()const
00078 {
00079 return data.cols();
00080 }
00081
00082 void Add(const JntArray& src1,const JntArray& src2,JntArray& dest)
00083 {
00084 dest.data=src1.data+src2.data;
00085 }
00086
00087 void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest)
00088 {
00089 dest.data=src1.data-src2.data;
00090 }
00091
00092 void Multiply(const JntArray& src,const double& factor,JntArray& dest)
00093 {
00094 dest.data=factor*src.data;
00095 }
00096
00097 void Divide(const JntArray& src,const double& factor,JntArray& dest)
00098 {
00099 dest.data=src.data/factor;
00100 }
00101
00102 void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest)
00103 {
00104 Eigen::Matrix<double,6,1> t=jac.data.lazyProduct(src.data);
00105 dest=Twist(Vector(t(0),t(1),t(2)),Vector(t(3),t(4),t(5)));
00106 }
00107
00108 void SetToZero(JntArray& array)
00109 {
00110 array.data.setZero();
00111 }
00112
00113 bool Equal(const JntArray& src1, const JntArray& src2,double eps)
00114 {
00115 if(src1.rows()!=src2.rows())
00116 return false;
00117 return src1.data.isApprox(src2.data,eps);
00118 }
00119
00120 bool operator==(const JntArray& src1,const JntArray& src2){return Equal(src1,src2);};
00121
00122
00123 }
00124
00125