Hello, I'm a newbie to C++ so excuse me if my question was trivial but
it is important to me.
I'm implementing a simple code to find the forward kinematics of a
robot:
#include "stdafx.h"
#include<iostre am>
#include<iomani p>
#include<fstrea m>
#include"math.h "
#include"string .h"
#include<string >
#include"wchar. h"
#include<set>
using namespace std;
using std::cin;
using std::cout;
using std::endl;
#include<cmath>
int main()
{
// Initializtion
int i,j,k,sum; // Used for
FOR loop declartion
double t1,t2,t3,t4,t5, t6; // Thetas in
radians
double pi = 3.14159265;
double alp[6]={pi/3,-pi/2,0,pi/4,-pi/6,pi/2}; // The size of the
array depends on the dof of the robot (user defines it)
double a[6]={0,0.23,0.46,0 .2,0.34,0.98};
double d[6]={0.24,0,0,0.1, 0.56,0};
int size=sizeof(a)/sizeof(a[0]);
// User input
cout<<"Enter t1 in degrees:";cin>> t1;
cout<<"Enter t2 in degrees:";cin>> t2;
cout<<"Enter t3 in degrees:";cin>> t3;
cout<<"Enter t4 in degrees:";cin>> t4;
cout<<"Enter t5 in degrees:";cin>> t5;
cout<<"Enter t6 in degrees:";cin>> t6;
double t[6]={t1*(pi/180),t2*(pi/180),t3*(pi/180),t4*(pi/180),t5*(pi/
180),t6*(pi/180)};
double*** T= new double**[size]; //pointer to pointer
// Initialize Transformation matrices as 3D matrices to contain the
rows and columns plus
// another dimension for the whole transformations (1-6)
for(i=0;i<size; i++){
T[i] = new double*[4];
for (j=0; j<4; j++) // j represents the number of columns and rows
T[i][j]= new double[4];
T[i][0][0]=cos(t[i]); T[i][0][1]=-sin(t[i]);T[i][0][2]=0;T[i][0]
[3]=a[i];
T[i][1][0]=sin(t[i])*cos(alp[i]);T[i][1]
[1]=cos(t[i])*cos(alp[i]);T[i][1][2]=-sin(alp[i]);T[i][1][3]=-
sin(alp[i])*d[i];
T[i][2][0]=sin(t[i])*sin(alp[i]);T[i][2]
[1]=cos(t[i])*sin(alp[i]);T[i][2][2]=cos(alp[i]);T[i][2]
[3]=cos(alp[i])*d[i];
T[i][3][0]=0;T[i][3][1]=0;T[i][3][2]=0;T[i][3][3]=1;
}
---------------------------------------------------------------------------------------------------------------------------------------------
T matrix basically contains all the parameters (a, alp,t, d) which are
defined before the loop. T[1] for example is a 4-by-4 matrix that has
elements associated with a[1],t[1],alp[1] & d[1]. Since size is 6 in
this case, I need to have T[1],T[2],......T[6].
This code works fine for me and I can display all of the T's
correctly. Now what I need to do is to multiply all of them to
resultant T=T[1]*T[2]*.....*T[6]. I wrote a simple code to multiply a
simple 2D matrix, but I'm not sure how to incorporate my third
dimension. Also, is there a simpler way(more efficient) to implement
the matrix instead of the way I defined it in the loop (element by
element)?
Thanks a million.
it is important to me.
I'm implementing a simple code to find the forward kinematics of a
robot:
#include "stdafx.h"
#include<iostre am>
#include<iomani p>
#include<fstrea m>
#include"math.h "
#include"string .h"
#include<string >
#include"wchar. h"
#include<set>
using namespace std;
using std::cin;
using std::cout;
using std::endl;
#include<cmath>
int main()
{
// Initializtion
int i,j,k,sum; // Used for
FOR loop declartion
double t1,t2,t3,t4,t5, t6; // Thetas in
radians
double pi = 3.14159265;
double alp[6]={pi/3,-pi/2,0,pi/4,-pi/6,pi/2}; // The size of the
array depends on the dof of the robot (user defines it)
double a[6]={0,0.23,0.46,0 .2,0.34,0.98};
double d[6]={0.24,0,0,0.1, 0.56,0};
int size=sizeof(a)/sizeof(a[0]);
// User input
cout<<"Enter t1 in degrees:";cin>> t1;
cout<<"Enter t2 in degrees:";cin>> t2;
cout<<"Enter t3 in degrees:";cin>> t3;
cout<<"Enter t4 in degrees:";cin>> t4;
cout<<"Enter t5 in degrees:";cin>> t5;
cout<<"Enter t6 in degrees:";cin>> t6;
double t[6]={t1*(pi/180),t2*(pi/180),t3*(pi/180),t4*(pi/180),t5*(pi/
180),t6*(pi/180)};
double*** T= new double**[size]; //pointer to pointer
// Initialize Transformation matrices as 3D matrices to contain the
rows and columns plus
// another dimension for the whole transformations (1-6)
for(i=0;i<size; i++){
T[i] = new double*[4];
for (j=0; j<4; j++) // j represents the number of columns and rows
T[i][j]= new double[4];
T[i][0][0]=cos(t[i]); T[i][0][1]=-sin(t[i]);T[i][0][2]=0;T[i][0]
[3]=a[i];
T[i][1][0]=sin(t[i])*cos(alp[i]);T[i][1]
[1]=cos(t[i])*cos(alp[i]);T[i][1][2]=-sin(alp[i]);T[i][1][3]=-
sin(alp[i])*d[i];
T[i][2][0]=sin(t[i])*sin(alp[i]);T[i][2]
[1]=cos(t[i])*sin(alp[i]);T[i][2][2]=cos(alp[i]);T[i][2]
[3]=cos(alp[i])*d[i];
T[i][3][0]=0;T[i][3][1]=0;T[i][3][2]=0;T[i][3][3]=1;
}
---------------------------------------------------------------------------------------------------------------------------------------------
T matrix basically contains all the parameters (a, alp,t, d) which are
defined before the loop. T[1] for example is a 4-by-4 matrix that has
elements associated with a[1],t[1],alp[1] & d[1]. Since size is 6 in
this case, I need to have T[1],T[2],......T[6].
This code works fine for me and I can display all of the T's
correctly. Now what I need to do is to multiply all of them to
resultant T=T[1]*T[2]*.....*T[6]. I wrote a simple code to multiply a
simple 2D matrix, but I'm not sure how to incorporate my third
dimension. Also, is there a simpler way(more efficient) to implement
the matrix instead of the way I defined it in the loop (element by
element)?
Thanks a million.
Comment