C
crazygrey
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<iostream>
#include<iomanip>
#include<fstream>
#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 = new double*[4];
for (j=0; j<4; j++) // j represents the number of columns and rows
T[j]= new double[4];
T[0][0]=cos(t); T[0][1]=-sin(t);T[0][2]=0;T[0]
[3]=a;
T[1][0]=sin(t)*cos(alp);T[1]
[1]=cos(t)*cos(alp);T[1][2]=-sin(alp);T[1][3]=-
sin(alp)*d;
T[2][0]=sin(t)*sin(alp);T[2]
[1]=cos(t)*sin(alp);T[2][2]=cos(alp);T[2]
[3]=cos(alp)*d;
T[3][0]=0;T[3][1]=0;T[3][2]=0;T[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<iostream>
#include<iomanip>
#include<fstream>
#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 = new double*[4];
for (j=0; j<4; j++) // j represents the number of columns and rows
T[j]= new double[4];
T[0][0]=cos(t); T[0][1]=-sin(t);T[0][2]=0;T[0]
[3]=a;
T[1][0]=sin(t)*cos(alp);T[1]
[1]=cos(t)*cos(alp);T[1][2]=-sin(alp);T[1][3]=-
sin(alp)*d;
T[2][0]=sin(t)*sin(alp);T[2]
[1]=cos(t)*sin(alp);T[2][2]=cos(alp);T[2]
[3]=cos(alp)*d;
T[3][0]=0;T[3][1]=0;T[3][2]=0;T[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.