26 using namespace Eigen;
28 JntSpaceInertiaMatrix::JntSpaceInertiaMatrix()
32 JntSpaceInertiaMatrix::JntSpaceInertiaMatrix(
int _size):
39 JntSpaceInertiaMatrix::JntSpaceInertiaMatrix(
const JntSpaceInertiaMatrix& arg):
44 JntSpaceInertiaMatrix& JntSpaceInertiaMatrix::operator = (
const JntSpaceInertiaMatrix& arg)
51 JntSpaceInertiaMatrix::~JntSpaceInertiaMatrix()
55 void JntSpaceInertiaMatrix::resize(
unsigned int newSize)
57 data.resize(newSize,newSize);
60 double JntSpaceInertiaMatrix::operator()(
unsigned int i,
unsigned int j)
const 65 double& JntSpaceInertiaMatrix::operator()(
unsigned int i,
unsigned int j)
70 unsigned int JntSpaceInertiaMatrix::rows()
const 75 unsigned int JntSpaceInertiaMatrix::columns()
const 81 void Add(
const JntSpaceInertiaMatrix& src1,
const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest)
83 dest.data=src1.data+src2.data;
86 void Subtract(
const JntSpaceInertiaMatrix& src1,
const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest)
88 dest.data=src1.data-src2.data;
91 void Multiply(
const JntSpaceInertiaMatrix& src,
const double& factor,JntSpaceInertiaMatrix& dest)
93 dest.data=factor*src.data;
96 void Divide(
const JntSpaceInertiaMatrix& src,
const double& factor,JntSpaceInertiaMatrix& dest)
98 dest.data=src.data/factor;
103 dest.
data=src.data.lazyProduct(vec.
data);
111 bool Equal(
const JntSpaceInertiaMatrix& src1,
const JntSpaceInertiaMatrix& src2,
double eps)
113 if(src1.rows()!=src2.rows()||src1.columns()!=src2.columns())
115 return (src1.data-src2.data).isZero(eps);
118 bool operator==(
const JntSpaceInertiaMatrix& src1,
const JntSpaceInertiaMatrix& src2){
return Equal(src1,src2);}
friend void SetToZero(JntArrayVel &array)
This class represents an fixed size array containing joint values of a KDL::Chain.
friend void Divide(const JntArrayVel &src, const double &factor, JntArrayVel &dest)
friend void Multiply(const JntArrayVel &src, const double &factor, JntArrayVel &dest)
friend void Subtract(const JntArrayVel &src1, const JntArrayVel &src2, JntArrayVel &dest)
friend bool Equal(const JntArrayVel &src1, const JntArrayVel &src2, double eps)
bool operator==(const Rotation &a, const Rotation &b)
friend void Add(const JntArrayVel &src1, const JntArrayVel &src2, JntArrayVel &dest)