Code:
H files.
class Link{
private:
double _a;
double _d;
double _alpha;
int _nPointsPerLink;
ColumnVector *_LocalPoints;
public:
Link() {
_a = _d = _alpha = _nPointsPerLink = 0;
_LocalPoints = NULL;
}
~Link(){
if (_LocalPoints)
delete [] _LocalPoints;
}
void set_d(double d) { _d = d; }
double get_d(void) const { return _d; }
void set_a(double a) { _a = a; }
double get_a(void) const { return _a; }
void set_alpha(double alpha) { _alpha = alpha; }
double get_alpha(void) const { return _alpha; }
int get_nPointsPerLink(void) const { return _nPointsPerLink;}
// The coordintes of lpcal pints in each robot reference system
void set_LocalPoints(const int n, ColumnVector * lp){
if (_LocalPoints)
delete [] _LocalPoints;
_nPointsPerLink = n;
_LocalPoints = new ColumnVector[n+1];
for ( int i=1; i<=n; i++ ){
_LocalPoints[i] = ColumnVector(3);
_LocalPoints[i] = lp[i-1];
}
}
// get a local point from the link (not the robot)
ColumnVector get_LocalPoints(int i) { return _LocalPoints[i];}
};
class PUMA560{
private:
const int _dof;
double *_qmax;
double *_qmin;
public:
PUMA560();
~PUMA560();
Link *link;
PUMA560Config *CurrentConfig;
int InvKin(ColumnVector & Pos, SquareMatrix & Ori, Matrix & VConfig);
int InvKin(ColumnVector & _Pos, Matrix & VConfig);
};
class PUMA560Config{
private:
PUMA560 & robot;
static const int _dof;
double *_qi;
ColumnVector _gROgOp[5][7]; // Define the vector from global coordinate to any significant
// point in the robot
ColumnVector _VectorToSignifPoint[5][7];
ColumnVector _DgROgOp[7][13+1];
SquareMatrix *_i_1_RotationMatrix_i;
SquareMatrix *_b_RotMat_i; // The Rotation Matrix from any Reference Frame with
// respect to Base Reference System
SquareMatrix *_g_RotMat_i; // The Rotation Matrix from any Reference Frame with
// respect to Global Reference System
SquareMatrix _zyzEulerB;
ColumnVector *_i_1_PostionVector_i;
ColumnVector *_g_Z_i;
ColumnVector *_gROgOi; // Define the vector from global coordinate to any frame
// reference system i in the robot
ColumnVector _iROiOj[6][7];
union {
struct { ColumnVector _PA, _PB, _PC, _P1, _PD, _PF, _P2, _PE, _P3, _PG, _P4, _PH, _PFF; };
//ColumnVector sig_pts[13];
};
union {
struct { ColumnVector *_DPA, *_DPB, *_DPC, *_DP1, *_DPD, *_DPF, *_DP2, *_DPE, *_DP3, *_DPG, *_DP4, *_DPH, *_DPFF; };
ColumnVector * sig_pts_drv[13];
};
public:
PUMA560Config(PUMA560 & r) : robot(r) {
_qi = new double[_dof+1];
_i_1_RotationMatrix_i = new SquareMatrix[_dof+1];
_b_RotMat_i = new SquareMatrix[_dof+1];
_g_RotMat_i = new SquareMatrix[_dof+1];
_i_1_PostionVector_i = new ColumnVector[_dof+1];
_g_Z_i = new ColumnVector[_dof+1];
_gROgOi = new ColumnVector[_dof+1];
for (int i=0; i<13; i++) {
sig_pts_drv[i] = new ColumnVector[_dof+1];
for (int j=1; j<=_dof; j++ )
sig_pts_drv[i][j] = ColumnVector(3);
}
}
~PUMA560Config(){
delete [] _qi;
delete [] _i_1_RotationMatrix_i;
delete [] _b_RotMat_i;
delete [] _g_RotMat_i;
delete [] _i_1_PostionVector_i;
delete [] _g_Z_i;
delete [] _gROgOi;
for (int i=0; i<13; i++)
delete [] sig_pts_drv[i];
}
void set_qi(ColumnVector &); // _qi = the angle between Xi-1 & Xi measured about Zi
void set_qi(double * q); // _qi = the angle between Xi-1 & Xi measured about Zi
double get_qi(int i){ return _qi[i]; }
int get_dof() { return _dof; }
void set_i_1_RotationMatrix_i();// Rotation Matrix and Posision Vector Construction with respect to
// Modified Denavit-Hartenberg Conventions!!!!
void zyzEuler(); // Euler Angels for the base of the robot (ZYZ).
void iROiOj(); // Define the vector ###(i)_R_O(i)_Oj###
void VectorToSignificantPoints(); // Define the vector from reference frame to significant
//points on the robotīs links
void gROgOp(); // Define the vector from global coordinate to the interesting points of the robot
void gROiOp(); // Define the partial derivative of G_R_OG_OP with respect to joints.
SquareMatrix getRotationMatrix(int i);
ColumnVector getPostionVector(int i);
ColumnVector getiROiOj(int i, int j);
ColumnVector get_iROiOj(int i, int j) { return _iROiOj[i][j]; }
ColumnVector getVectorToSignificantPoints(int i, int j) { return _VectorToSignifPoint[i][j];}
ColumnVector get_gROgOp(int j, int i) { return _gROgOp[j][i]; }
ColumnVector get_DgROgOp(int i, int j) { return _DgROgOp[i][j]; }
SquareMatrix get_b_RotMat_i(int i) { return _b_RotMat_i[i]; }
SquareMatrix get_g_RotMat_i(int i) { return _g_RotMat_i[i]; }
SquareMatrix getzyzEulerB() { return _zyzEulerB; }
ColumnVector getEndEffectorPos() { return _PFF; }
ColumnVector get_PD() { return _PD; }
ColumnVector get_PF() { return _PF; }
ColumnVector get_PH() { return _PH; }
bool CheckObstacle(Obstacle &);
};
Some Cpp files.
PUMA560::~PUMA560()
{
delete [] link;
delete CurrentConfig;
delete [] _qmin;
delete [] _qmax;
}
PUMA560::PUMA560() : _dof(6) {
CurrentConfig = new PUMA560Config(*this);
link = new Link[_dof+1];
_qmin = new double[_dof+1];
_qmin[1] = -2.793; _qmin[2] = -3.7525; _qmin[3] = -0.7854;
_qmin[4] = -2.4435; _qmin[5] = -1.7453; _qmin[6] = -4.6426;
_qmax = new double[_dof+1];
_qmax[1] = 2.793; _qmax[2] = 0.6109; _qmax[3] = 3.9270;
_qmax[4] = 2.4435; _qmax[5] = 1.7453; _qmax[6] = 4.6426;
// Ai_1 = the distance from Zi-1 to Zi measured along Xi-1
double a_i_1[] = {0.0, 0.0, 0.4318, -0.02032, 0.0, 0.0};
// ALFAi_1 = the angle between Zi-1 & Zi measured about Xi-1
double alpha_i_1[] = {0.0, -90.0, 0.0, 90.0, -90.0, 90.0};
// Di = the distance from Xi-1 to Xi measured along Zi
double di[] = {0.0, 0.0, 0.14909, 0.4318, 0.0, 0.0};
// Number of Significant points per link
int nPointsPerLink[] = {2, 2, 4, 2, 2, 1};
// The coordintes of local pints in each robot reference system
double pts [] = { 0.0, 0.0, -0.672, 0.0, 0.0, 0.0,
0.0, 0.0, 0.255, -0.229, 0.0, 0.255,
0.0, 0.0, 0.105, 0.0, -0.351, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, -0.081, 0.0, 0.0, -0.081,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.267 }; // 3'ayaretha mo2aqatan, lazem tkon 0.267
double * c_pts = pts;
for ( int i=0; i<_dof; i++){
link[i].set_a(a_i_1[i]);
link[i].set_alpha(alpha_i_1[i]);
link[i+1].set_d(di[i]);
ColumnVector * v = new ColumnVector[ nPointsPerLink[i] ];
for (int j=0; j<nPointsPerLink[i]; j++) {
v[j] = ColumnVector(3);
v[j].setElements(c_pts, 3);
c_pts += 3;
}
link[i+1].set_LocalPoints(nPointsPerLink[i], v);
delete [] v;
}
double q[] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
CurrentConfig->set_qi(q);
}
// _qi = the angle between Xi-1 & Xi measured about Zi
void PUMA560Config::set_qi(ColumnVector & v){
double *q = new double [v.getnRows()];
for (int i=0; i<v.getnRows(); i++)
q[i] = v(i+1);
set_qi(q);
delete [] q;
}
void PUMA560Config::set_qi(double * q){
for (int i=1; i<=_dof; i++)
_qi[i] = q[i-1];
set_i_1_RotationMatrix_i();
zyzEuler();
iROiOj();
VectorToSignificantPoints();
gROgOp();
gROiOp();
}
int main(){
PUMA560 robot;
// some lines
return 0;
}
when i debug it, the _LocalPoints in robot variable are not set.
Code:
class Matrix{
private:
int _nColumns;
int _nRows;
double *_data;
public:
// Constructor
Matrix() {_nRows = 0; _nColumns = 0; _data=NULL;}
// Copy Constructor
Matrix(const Matrix & m){
_nRows = m._nRows;
_nColumns = m._nColumns;
if (_nColumns && _nRows) {
_data = new double [_nColumns * _nRows];
for (int i=0; i<_nRows*_nColumns; i++)
_data[i] = m._data[i];
}else
_data = NULL;
}
// Constructor: Creates a matrix and sets all elements to 0.
// Constructor: Creates a matrix with all values equal to value.
Matrix(int r, int c, double x=0){
if ( r<0 || c<0 )
throw InvalidIndex();
_nRows = r;
_nColumns = c;
if ( _nRows && _nColumns ){
_data = new double [_nRows*_nColumns];
for ( int i=0; i<_nRows*_nColumns; i++)
_data[i] = x;
}else
_data = NULL;
}
// Constructor: builds a matrix from a given array.
Matrix(int r, int c, double *x){
if ( r<0 || c<0 )
throw InvalidIndex();
_nRows = r;
_nColumns = c;
if ( _nRows && _nColumns ){
_data = new double [_nRows*_nColumns];
for ( int i=0; i<_nRows*_nColumns; i++)
_data[i] = x[i];
}else
_data = NULL;
}
// Destructor
~Matrix(){
if ( _data )
delete [] _data;
}
void setElement( int i, int j, double e); // Set an Element in the Matrix
void setElements(double *array, int n); // Set an Array in the Matrix
double getElement(int i, int j) const; // Get an Element from the Matrix
void resize (int r, int c); // Resizes the Matrix, initializes new values to zero.
double & operator () (int i, int j) const;
/* Matrix & operator =(Matrix & m);*/ // Sets Matrix equal to another matrix.
Matrix & operator =(const Matrix & m); // Sets Matrix equal to another matrix.
Matrix & operator =(double x);
bool operator ==(Matrix & m);
bool operator !=(Matrix & m) { return !((*this) == m); }
Matrix operator +(Matrix &m); // Matrix Addition
Matrix operator -(Matrix &m); // Matrix Substraction
Matrix operator *(Matrix & m); // Matrix Multiplication
Matrix operator +=(Matrix &m) { return (*this) = (*this) + m; }
Matrix operator -=(Matrix &m) { return (*this) = (*this) - m; }
Matrix operator *=(Matrix & m) { return (*this) = (*this) * m; }
Matrix operator ~(); // Matrix Transpose
Matrix operator * (double n); // Matrix Scalar Multiplication mat * n
Matrix operator *=(double n) { return (*this) = (*this) * n; }
ColumnVector getColumn(int n) const;
RowVector getRow(int m) const;
ColumnVector getRowAsCol(int m) const;
void setColAsRow(ColumnVector col, int i) const;
int getnRows() const { return _nRows; }
int getnColumns() const { return _nColumns; }
void zero (); // Replaces all values in the Matrix with zero.
void ones (); // Replaces all values in the Matrix with one.
};
class ColumnVector : public Matrix{
public:
Matrix & operator = (const Matrix & m) {
if (m.getnColumns() != 1)
cout << "Error: nColumns must be equal 1";
return (*(Matrix*)this) = m;
}
Matrix & operator = (double x) {
return (*(Matrix*)this) = x;
}
ColumnVector( int n=0 ) : Matrix (n,1) { }
ColumnVector( const Matrix & m) {
if (m.getnColumns() != 1)
cout << "Error: nColumns must be equal 1";
*this = m;
}
double getElement(int i) const { return Matrix::getElement(i,1);}
void setElement(int i, double n) { Matrix::setElement(i,1,n); }
void setElements(double * array, int n) { Matrix::setElements(array, n); }
double & operator () ( int i ) const { return (*(Matrix *)this)(i,1); }
};
// Cpp files
Matrix & Matrix::operator =(const Matrix & m)
{
if (_data)
delete [] _data;
_nRows = m._nRows;
_nColumns = m._nColumns;
if (!_nRows || !_nColumns )
throw InvalidMatrixSize();
_data = new double[_nRows*_nColumns];
for (int i=0; i<_nRows*_nColumns ; i++)
_data[i] = m._data[i]; // sometimes it stucks here.
return *this;
}
If anyone can help,,