/* RoboticsLab, Copyright 2008-2009 SimLab Co., Ltd. All rights reserved.
 *
 * This library is commercial and cannot be redistributed, and/or modified
 * WITHOUT ANY ALLOWANCE OR PERMISSION OF SimLab Co., LTD.
 */
#include "stdafx.h"

#include "rMath/rMath.h"
using namespace rMath;

#include "rCommand/rCmdDefine.h"
#include "rxSDK/rxSDK.h"
#include "rCustomDraw.h"

#ifdef USING_NAMESPACE_RTERM
#include "rTerm/rTerm.h"
using namespace rTerm;
#endif

#include <map>
#include "math.h"
#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
#include <malloc.h>

bool bContact = true;

bool bQuit = false;
bool bRun = false;

#ifdef USING_NAMESPACE_RTERM
// Defining a macro, RPLAYER_AUTO_START, makes your program start rPlayer automatically.
// If you don't want rPlayer to be started automatically, uncomment the code below.
#define RPLAYER_AUTO_START
#endif

FILE* f;

//Global Variables

float **p, **w, **y, **rtemp5, **rtemp6, **rtemp7, **rtemp8, **rtemp9;
float *r, *v, *u, *rtemp, *rtemp2, *rtemp3, *rtemp4;

float *H1, *H2, *H3, *H4, *H5, *H6, *H7;
float *C1, *C2, *C3, *C4, *C5, *C6, *C7;

float **H, **H_task, **C, **C_task;
float **J_Jtrans, **J_inv, **Jtrans_inv;
float **inertemp1, **inertemp2, **cortemp1, **cortemp2, **cortemp3, **cortemp4;

float **J, **Jtrans, **Jdot;

float **A1, **A2, **A3, **A4, **A5, **A6, **A7;
float **T1, **T2, **T3, **T4, **T5, **T6, **T7;
float *J1_upperhalf, *J2_upperhalf, *J3_upperhalf, *J4_upperhalf, *J5_upperhalf, *J6_upperhalf, *J7_upperhalf;
float *Z0dot, *Z1dot, *Z2dot, *Z3dot, *Z4dot, *Z5dot, *Z6dot;
float *Udot1_0, *Udot2_1, *Udot3_2, *Udot4_3, *Udot5_4, *Udot6_5, *Udot7_6;
float *J1dot_upperhalf, *J2dot_upperhalf, *J3dot_upperhalf, *J4dot_upperhalf, *J5dot_upperhalf, *J6dot_upperhalf, *J7dot_upperhalf;

float **RI;

float *temp1, *temp2, *temp3, *temp4, *temp5, *temp6, *temp7, *temp8, *temp9, *temp10, *temp11, *temp12, *temp13, *temp14, *temp15, *temp16, *temp17, *temp18, *temp19, *temp20, *temp21, *temp22, *temp23, *temp24, *temp25, *temp26;

float *term1, *term2, *term3, *term4, *term5, *term6, *term7, *term8, *term9, *term10, *term11, *term12, *term13, *term14;

float q[7] = {{0}, {0}, {0}, {0}, {0}, {0}, {0}};
float dq[7] = {{0}, {0}, {0}, {0}, {0}, {0}, {0}};

float tau[7] = {{0}, {0}, {0}, {0}, {0}, {0}, {0}};

float null_v[7] = {{0}, {0}, {0}, {0}, {0}, {0}, {0}};

float V1[7] = {{1}, {0}, {0}, {0}, {0}, {0}, {0}};
float V2[7] = {{0}, {1}, {0}, {0}, {0}, {0}, {0}};
float V3[7] = {{0}, {0}, {1}, {0}, {0}, {0}, {0}};
float V4[7] = {{0}, {0}, {0}, {1}, {0}, {0}, {0}};
float V5[7] = {{0}, {0}, {0}, {0}, {1}, {0}, {0}};
float V6[7] = {{0}, {0}, {0}, {0}, {0}, {1}, {0}};
float V7[7] = {{0}, {0}, {0}, {0}, {0}, {0}, {1}};

float l1 = 0.140;
float l2 = 0.130;
float l3 = 0.180;
float l4 = 0.120;
float l5 = 0.180;
float l6 = 0;
float l7 = 0.100;

float m[7] = {1,4,4,2.8,2.8,1.8,1.8};

float I1_33 = m[0]*(l1/2)*(l1/2);
float I2_33 = m[1]*(l2/2)*(l2/2);
float I3_33 = m[2]*(l3/2)*(l3/2);
float I4_33 = m[3]*(l4/2)*(l4/2);
float I5_33 = m[4]*(l5/2)*(l5/2);
float I6_33 = m[5]*(l6/2)*(l6/2);
float I7_33 = m[6]*(l7/2)*(l7/2);


float I1[3][3]={{0.00409, 0.0000157, -0.00000000552},
				{0.0000157, 0.00306, -0.000000168},
				{-0.00000000552, -0.000000168, 0.00277}};

float I2[3][3]={{0.011567, 0.0000157, 0.00000000552},
				{0.0000157, 0.011002, -0.000000168},
				{0.00000000552, -0.000000168, 0.004802}};

float I3[3][3]={{0.012857, -0.00000735, -0.00000000345},
				{-0.00000735, 0.014418, -0.000000733},
				{-0.00000000345, -0.000000733, 0.004409}};

float I4[3][3]={{0.006152, -0.00000735, -0.00000000345},
				{-0.00000735, 0.0062038, -0.000000733},
				{-0.00000000345, -0.000000733, 0.001934}};

float I5[3][3]={{0.0060550, -0.00000031588, -0.0000002845},
				{-0.00000031588, 0.006124, 0.000000001338},
				{-0.0000002845, 0.000000001338, 0.001714}};

float I6[3][3]={{0.00089381, 0.0000004356, 0},
				{0.0000004356, 0.0007706, 0},
				{0, 0, 0.0008692}};

float I7[3][3]={{0.001808, 0.0000018115, -0.000000004334},
				{0.0000018115, 0.0014186, -0.00000092181},
				{-0.000000004334, -0.00000092181, 0.00072028}};

struct link_nomen
{
	float Z[3];
	float r_end[3];
	float r_com[3];
	float **I;
	float **R;
	float **Rtrans;
	float *omega;
	float *omega_a;
	float *omegadot;
	float *Oframeddot;
	float *Bframeddot;
	float *force;
	float *tau;
	
} link[7];

float *omega_0, *omega_1, *omega_2, *omega_3, *omega_4, *omega_5, *omega_6;
float *Jdottemp1, *Jdottemp2, *Jdottemp3, *Jdottemp4, *Jdottemp5, *Jdottemp6;

float Xd = -0.4;
float Yd = 0.0;
float Zd = 0.5;	

float k=0;

//  Related to DOB

float input_force[3] = {0, 0, 0};
float input_force_1T[3] = {0, 0, 0};
float input_force_2T[3] = {0, 0, 0};
float input_force_3T[3] = {0, 0, 0};

float final_force[3] = {0, 0, 0};
float final_force_1T[3] = {0, 0, 0};
float final_force_2T[3] = {0, 0, 0};
float final_force_3T[3] = {0, 0, 0};

float X=0;
float X_1T=0;
float X_2T=0;
float X_3T=0;

float Y=0;
float Y_1T=0;
float Y_2T=0;
float Y_3T=0;

float Z=0;
float Z_1T=0;
float Z_2T=0;
float Z_3T=0;

float Xvel=0;
float Xvel_1T=0;
float Xvel_2T=0;
float Xvel_3T=0;

float Yvel=0;
float Yvel_1T=0;
float Yvel_2T=0;
float Yvel_3T=0;

float Zvel=0;
float Zvel_1T=0;
float Zvel_2T=0;
float Zvel_3T=0;

float Xangvel=0;
float Xangvel_1T=0;
float Xangvel_2T=0;
float Xangvel_3T=0;

float Yangvel=0;
float Yangvel_1T=0;
float Yangvel_2T=0;
float Yangvel_3T=0;

float Zangvel=0;
float Zangvel_1T=0;
float Zangvel_2T=0;
float Zangvel_3T=0;

float H_task_1T[6][6] = {{0,0,0,0,0,0},
						 {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0}};

float H_task_2T[6][6] = {{0,0,0,0,0,0},
						 {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0}};

float H_task_3T[6][6] = {{0,0,0,0,0,0},
						 {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0}};


float C_task_1T[6][6] = {{0,0,0,0,0,0},
						 {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0}};

float C_task_2T[6][6] = {{0,0,0,0,0,0},
						 {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0}};

float C_task_3T[6][6] = {{0,0,0,0,0,0},
						 {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0},
                         {0,0,0,0,0,0}};

float X0 = -0.3;
float Y0 = 0;
float Z0 = 0.35;

float posdel0norm = sqrt((X0-Xd)*(X0-Xd)+ (Y0-Yd)*(Y0-Yd) + (Z0-Zd)*(Z0-Zd));

float t = 0;
float T = 0.001;

/////////////

void OnKeyQuit(void* data)
{
	PHYSICS_WORLD->deactivateWorld();
	bQuit = true;
}

void OnKeyRun(void* data)
{
	bRun = !bRun;
	if(bRun)
		PHYSICS_WORLD->activateWorld();
	else
		PHYSICS_WORLD->deactivateWorld();
}

void SetKeyboardHandler()
{
	PHYSICS_WORLD->addKeyboardEvent(VK_Q, OnKeyQuit);
	PHYSICS_WORLD->addKeyboardEvent(VK_TAB, OnKeyRun);
	printf("To run/pause simulation, press TAB key.\n");
	printf("To quit simulation, press 'q' key.\n");
	PHYSICS_WORLD->printLogMessage(_T("To run/pause simulation, press TAB key."));
	PHYSICS_WORLD->printLogMessage(_T("To quit simulation, press 'q' key."));
}

// User Defined Functions

float **newpointer(float **a, int j, int k)
{
	a = new float* [j];
	
	for (int i=0; i<j; i++)
	*(a+i) = new float[k];

	return a;
}

float *newsinglepointer(float *a, int i)
{
	a = new float[i];
	return a;
}

void freepointer(float **a, int j)
{
	int i;
	for(i = 0; i < j; i++)
	delete [] *(a+i);
	delete [] a;	
	a=0;
}

void freesinglepointer(float *a)
{
	delete []a;	
	a=0;
}

float **MatrixProduct(float **a, float **b)
{
	int i;
	int j;
	int k;

	p = newpointer(p,4,4);

 	for(i=0;i<=3;i++)
	{
		for(j=0;j<=3;j++)
		{
			p[i][j] = 0;
		}
	}
	 

	 for(i=0;i<=3;i++)
	 {
		for(k=0;k<=3;k++)
		{
			for(j=0;j<=3;j++)
			{
				p[i][k] = p[i][k] + a[i][j]*b[j][k];
			}
		}
	 }
     
	 return p;
}

float *MatrixProduct2(float **a, float *b)
{
	int i;
	int j;
	int k;

	v = newsinglepointer(v,3);

	for(i=0;i<3;i++)
	{
		v[i] = 0;
	}


	for(i=0;i<=2;i++)
	{
		for(j=0;j<=2;j++)
			{
				v[i] = v[i] + a[i][j]*b[j];
			}
	}

	return v;
}

float **MatrixProduct3(float **a, float **b, float a_nrows, float a_ncols, float b_nrows, float b_ncols)
{
	int i;
	int j;
	int k;

	w = newpointer(w,a_nrows,b_ncols);

	for(i=0;i<a_nrows;i++)
	{
		for(j=0;j<b_ncols;j++)
		{
			w[i][j] = 0;
		}
	}


	for(i=0;i<a_nrows;i++)
	{
		for(k=0;k<b_ncols;k++)
		{
			for(j=0;j<a_ncols;j++)
			{
				w[i][k] = w[i][k] + a[i][j]*b[j][k];
			}
		}
	}

	return w;
}

float detrm(float a[6][6], float k)
{
	float s=1,det=0,rtemp5[6][6];
	int i,j,m,n,c;

	if(k==1)
	{
		return(a[0][0]);
	}
	else
	{
		det=0;
		for(c=0;c<k;c++)
		{
			m=0;
			n=0;
			for(i=0;i<k;i++)
			{
				for(j=0;j<k;j++)
				{
					rtemp5[i][j]=0;
					if(i!=0&&j!=c)
					{
						rtemp5[m][n]=a[i][j];
						if(n<(k-2))
							n++;
						else
						{
							n=0;
							m++;
						}
					}
				}
			}
			det=det+s*(a[0][c]*detrm(rtemp5,k-1));
			s=-1*s;
		}
	}

	return det;
}

float **trans(float num[6][6], float fac[6][6], float r)
{
	int i,j;
	float b[6][6],d;
	
	rtemp6 = newpointer(rtemp6,6,6);

	for(i=0;i<6;i++)
	{
		for(j=0;j<6;j++)
		{
			rtemp6[i][j]=0;
		}
	}

	for(i=0;i<r;i++)
	{
		for(j=0;j<r;j++)
		{
			b[i][j]=fac[j][i];
		}
	}

	d=detrm(num,r);
	for(i=0;i<r;i++)
	{
		for(j=0;j<r;j++)
		{
			if (d==1 || b[i][j]==0)
				rtemp6[i][j] = b[i][j];
			else
				rtemp6[i][j] = b[i][j]/d;
		}
	}

	return rtemp6;
}

float **inverse_mat(float **a, float f)
{
	int p,q,m,n,i,j;
	float par = -1;

	float rtemp7[6][6], rtemp8[6][6], num[6][6];

	for(i=0;i<6;i++)
	{
		for(j=0;j<6;j++)
		{
			num[i][j]=a[i][j];
		}
	}

	rtemp9 = newpointer(rtemp9,6,6);
	
	for(i=0;i<6;i++)
	{
		for(j=0;j<6;j++)
		{
			rtemp9[i][j]=0;
		}
	}

	for(q=0;q<f;q++)
	{
		for(p=0;p<f;p++)
		{
			m=0;
			n=0;
			for(i=0;i<f;i++)
			{
				for(j=0;j<f;j++)
				{
					rtemp7[i][j]=0;
					if(i!=q&&j!=p)
					{
						rtemp7[m][n]=num[i][j];
						if(n<(f-2))
							n++;
						else
						{
							n=0;
							m++;
						}
					}
				}
			}
			rtemp8[q][p]=pow(par,(q+p))*detrm(rtemp7,f-1);
		}
	}
	rtemp9 = trans(num,rtemp8,f);

	return rtemp9;
}

void assign(float **a, float b[4][4])
{
	int i;
	int j;

	for(i=0;i<=3;i++)
	{
		for(j=0;j<=3;j++)
		{
			a[i][j]  = b[i][j];
		}
	}
}

void assign2(float **a, float b[3][3])
{
	int i;
	int j;

	for(i=0;i<=2;i++)
	{
		for(j=0;j<=2;j++)
		{
			a[i][j]  = b[i][j];
		}
	}
}

void print(float **a)
{
	int i;
	int j;

	for(i=0;i<=3;i++)
	{
		for(j=0;j<=3;j++)
		{
			printf("%.2f\t", a[i][j]);
		}
		printf("\n");
	}
	getch();
}

float *CrossProduct(float a[], float b[])
{
	r = newsinglepointer(r,3);

	r[0] = a[1]*b[2] - a[2]*b[1];
	r[1] = a[2]*b[0] - a[0]*b[2];
	r[2] = a[0]*b[1] - a[1]*b[0];

	return r;
}

float *AddVector(float a[], float b[])
{
	rtemp4 = newsinglepointer(rtemp4,3);

	rtemp4[0] = a[0]+b[0];
	rtemp4[1] = a[1]+b[1];
	rtemp4[2] = a[2]+b[2];

	return rtemp4;
}

float *AddVector(float a[], float b[], float c[])
{
	rtemp = newsinglepointer(rtemp,3);

	rtemp[0] = a[0]+b[0]+c[0];
	rtemp[1] = a[1]+b[1]+c[1];
	rtemp[2] = a[2]+b[2]+c[2];

	return rtemp;
}

float *SubVector(float a[], float b[])
{
	rtemp3 = newsinglepointer(rtemp3,3);

	rtemp3[0] = a[0]-b[0];
	rtemp3[1] = a[1]-b[1];
	rtemp3[2] = a[2]-b[2];

	return rtemp3;
}

float **SubMatrix(float **a, float **b)
{
	int i;
	int j;
	
	y = newpointer(y,6,6);

	for(i=0;i<6;i++)
	{
		for(j=0;j<6;j++)
		{
			y[i][j] = 0;
		}
	}


	for(i=0;i<6;i++)
	{
		for(j=0;j<6;j++)
			{
				y[i][j] = a[i][j] - b[i][j];
			}
	}

	return y;
}

float *ScalarProduct(float a, float b[])
{
	rtemp2 = newsinglepointer(rtemp2,3);

	rtemp2[0] = a*b[0];
	rtemp2[1] = a*b[1];
	rtemp2[2] = a*b[2];

	return rtemp2;
}

//Modified Newton-Euler Inverse Dynamics Algorithm --- NE*(_alpha_0_0_0_1)

float *inverse_dyn(float qtemp[], float dqtemp[], float dqatemp[], float ddqtemp[])
{

	u = newsinglepointer(u,7);

	for(int i=0;i<7;i++)
	{
		link[i].I = newpointer(link[i].I,3,3);
        link[i].R = newpointer(link[i].R,3,3);
		link[i].Rtrans = newpointer(link[i].Rtrans,3,3);
		link[i].omega = newsinglepointer(link[i].omega, 3);
		link[i].omega_a = newsinglepointer(link[i].omega_a, 3);
		link[i].omegadot = newsinglepointer(link[i].omegadot, 3);
		link[i].Oframeddot = newsinglepointer(link[i].Oframeddot, 3);
		link[i].Bframeddot = newsinglepointer(link[i].Bframeddot, 3);
		link[i].force = newsinglepointer(link[i].force, 3);
		link[i].tau = newsinglepointer(link[i].tau, 3);
	}

	temp1 = newsinglepointer(temp1, 3);
	temp2 = newsinglepointer(temp2, 3);
	temp3 = newsinglepointer(temp3, 3);
	temp4 = newsinglepointer(temp4, 3);
	temp5 = newsinglepointer(temp5, 3);
	temp6 = newsinglepointer(temp6, 3);
	temp7 = newsinglepointer(temp7, 3);
	temp8 = newsinglepointer(temp8, 3);
	temp9 = newsinglepointer(temp9, 3);
	temp10 = newsinglepointer(temp10, 3);
	temp20 = newsinglepointer(temp20, 3);
	temp21 = newsinglepointer(temp21,3);
	temp22 = newsinglepointer(temp22,3);
	temp23 = newsinglepointer(temp23,3);
	temp24 = newsinglepointer(temp24,3);
	
	temp11 = newsinglepointer(temp11, 3);
	temp12 = newsinglepointer(temp12, 3);
	temp13 = newsinglepointer(temp13, 3);
	temp14 = newsinglepointer(temp14, 3);
	temp15 = newsinglepointer(temp15, 3);
	temp16 = newsinglepointer(temp16, 3);
	temp17 = newsinglepointer(temp17, 3);
	temp18 = newsinglepointer(temp18, 3);
	temp19 = newsinglepointer(temp19, 3);
	temp25 = newsinglepointer(temp24,3);
	temp26 = newsinglepointer(temp25,3);

	for(int k=0;k<7;k++)
	{
		for(int i=0;i<3;i++)
		{
			for(int j=0;j<3;j++)
			{
				if (k==0)
					link[k].R[i][j]=A1[i][j];

				else if(k==1)
					link[k].R[i][j]=A2[i][j];

				else if(k==2)
					link[k].R[i][j]=A3[i][j];

				else if(k==3)
					link[k].R[i][j]=A4[i][j];

				else if(k==4)
					link[k].R[i][j]=A5[i][j];

				else if(k==5)
					link[k].R[i][j]=A6[i][j];

				else 
					link[k].R[i][j]=A7[i][j];
			}
				
		}
	}

	for(int k=0; k<7; k++)
		{
			for (int i=0; i<3; i++)
				{
					for (int j=0; j<3; j++)
						{
							link[k].Rtrans[j][i] = link[k].R[i][j];
						}
				}
		}

	RI = newpointer(RI,3,3);
	
	float RI_I[3][3] = {{1, 0, 0},
				        {0, 1, 0},
	                    {0, 0, 1}};

	assign2(RI, RI_I);
	

	// r(i-1,i)

	link[0].r_end[0]=0;
	link[0].r_end[1]=l1;
	link[0].r_end[2]=0;

	link[1].r_end[0]=0;
	link[1].r_end[1]=0;
	link[1].r_end[2]=0;

	link[2].r_end[0]=0;
	link[2].r_end[1]=(l2+l3);
	link[2].r_end[2]=0;

	link[3].r_end[0]=0;
	link[3].r_end[1]=0;
	link[3].r_end[2]=0;

	link[4].r_end[0]=0;
	link[4].r_end[1]=(l4+l5);
	link[4].r_end[2]=0;

	link[5].r_end[0]=l6;
	link[5].r_end[1]=0;
	link[5].r_end[2]=0;


	link[6].r_end[0]=l7;
	link[6].r_end[1]=0;
	link[6].r_end[2]=0;

	// r(i,B)

	link[0].r_com[0]=0;
	link[0].r_com[1]=-0.07;
	link[0].r_com[2]=0;

	link[1].r_com[0]=0;
	link[1].r_com[1]=0;
	link[1].r_com[2]=0.065;

	link[2].r_com[0]=0;
	link[2].r_com[1]=-0.09;
	link[2].r_com[2]=0;

	link[3].r_com[0]=0;
	link[3].r_com[1]=0;
	link[3].r_com[2]=0.06;

	link[4].r_com[0]=0;
	link[4].r_com[1]=-0.09;
	link[4].r_com[2]=0;

	link[5].r_com[0]=0;
	link[5].r_com[1]=0;
	link[5].r_com[2]=0;

	link[6].r_com[0]=-0.05;
	link[6].r_com[1]=0;
	link[6].r_com[2]=0;

	//z(i-1)

	for(int i=0;i<7;i++)
	{
		link[i].Z[0]=0;
		link[i].Z[1]=0;
		link[i].Z[2]=1;
	}

	//Initialization

	float omega0[3]={{0},{0},{0}};
	float omega_a0[3]={{0},{0},{0}};
	float omegadot0[3]={{0},{0},{0}};
	float Oframeddot0[3]={{0},{0},{0}};

	
	//I(i)

	assign2(link[0].I, I1);
	assign2(link[1].I, I2);
	assign2(link[2].I, I3);
	assign2(link[3].I, I4);
	assign2(link[4].I, I5);
	assign2(link[5].I, I6);
	assign2(link[6].I, I7);

  
   // Forward Iteration

   // For Link 1

   temp1 = ScalarProduct(dqtemp[0],link[0].Z);
   temp21=AddVector(omega0, temp1); // AVR
   
   link[0].omega = MatrixProduct2(link[0].Rtrans, temp21); //Transformation to proper reference frame (Refer Siavicco / Craig)

   temp20 = ScalarProduct(dqatemp[0],link[0].Z);
   temp22=AddVector(omega_a0, temp20); //AVR*

   link[0].omega_a = MatrixProduct2(link[0].Rtrans, temp22); //Transformation to proper reference frame (Refer Siavicco / Craig)

   temp2 = ScalarProduct(dqtemp[0], omega_a0);
   temp3 = CrossProduct(temp2, link[0].Z);
   temp4 = ScalarProduct(ddqtemp[0], link[0].Z);
   temp23 = AddVector(omegadot0, temp4, temp3); // AAR*_0

   link[0].omegadot = MatrixProduct2(link[0].Rtrans, temp23); //Transformation to proper reference frame (Refer Siavicco / Craig)

   temp5 = CrossProduct(link[0].omega_a, link[0].r_end);
   temp6 = CrossProduct(link[0].omega, temp5);
   temp7 = CrossProduct(link[0].omegadot, link[0].r_end);
   temp24 = MatrixProduct2(link[0].Rtrans, Oframeddot0); //Transformation to proper reference frame (Refer Siavicco / Craig)

   link[0].Oframeddot=AddVector(temp24, temp7, temp6); // LAR'*_0

   temp8 = CrossProduct(link[0].omegadot, link[0].r_com);
   temp9 = CrossProduct(link[0].omega_a, link[0].r_com);
   temp10 = CrossProduct(link[0].omega, temp9);

   link[0].Bframeddot=AddVector(link[0].Oframeddot, temp8, temp10);	//LAR*_0

   
   freesinglepointer(temp10);
   freesinglepointer(temp9);
   freesinglepointer(temp8);
   freesinglepointer(temp24);
   freesinglepointer(temp7);
   freesinglepointer(temp6);
   freesinglepointer(temp5);
   freesinglepointer(temp23);
   freesinglepointer(temp4);
   freesinglepointer(temp3);
   freesinglepointer(temp2);
   freesinglepointer(temp22);
   freesinglepointer(temp20);
   freesinglepointer(temp21); 
   freesinglepointer(temp1);


   	// For all other links

  for(int i=1;i<7;i++)
  {
	  temp1 = newsinglepointer(temp1, 3);
	  temp2 = newsinglepointer(temp2, 3);
	  temp3 = newsinglepointer(temp3, 3);
	  temp4 = newsinglepointer(temp4, 3);
	  temp5 = newsinglepointer(temp5, 3);
	  temp6 = newsinglepointer(temp6, 3);
	  temp7 = newsinglepointer(temp7, 3);
	  temp8 = newsinglepointer(temp8, 3);
	  temp9 = newsinglepointer(temp9, 3);
	  temp10 = newsinglepointer(temp10, 3);
	  temp20 = newsinglepointer(temp20, 3);
	  temp21 = newsinglepointer(temp21,3);
	  temp22 = newsinglepointer(temp22,3);
	  temp23 = newsinglepointer(temp23,3);
	  temp24 = newsinglepointer(temp24,3);
	  
	  temp1 = ScalarProduct(dqtemp[i],link[i].Z);
	  temp21=AddVector(link[i-1].omega, temp1); // AVR

	  link[i].omega = MatrixProduct2(link[i].Rtrans, temp21); //Transformation to proper reference frame (Refer Siavicco / Craig)

	  temp20 = ScalarProduct(dqatemp[i],link[i].Z);
	  temp22 = AddVector(link[i-1].omega_a, temp20); //AVR*

	  link[i].omega_a = MatrixProduct2(link[i].Rtrans, temp22); //Transformation to proper reference frame (Refer Siavicco / Craig)

      temp2 = ScalarProduct(dqtemp[i], link[i-1].omega_a);
	  temp3 = CrossProduct(temp2, link[i].Z);
	  temp4 = ScalarProduct(ddqtemp[i], link[i].Z);
	  temp23 = AddVector(link[i-1].omegadot, temp4, temp3); // AAR*_0

	  link[i].omegadot = MatrixProduct2(link[i].Rtrans, temp23); //Transformation to proper reference frame (Refer Siavicco / Craig)

	  temp5 = CrossProduct(link[i].omega_a, link[i].r_end);
	  temp6 = CrossProduct(link[i].omega, temp5);
	  temp7 = CrossProduct(link[i].omegadot, link[i].r_end);
	  temp24 = MatrixProduct2(link[i].Rtrans, link[i-1].Oframeddot); //Transformation to proper reference frame (Refer Siavicco / Craig)

	  link[i].Oframeddot=AddVector(temp24, temp7, temp6); // LAR'*_0

	  temp8 = CrossProduct(link[i].omegadot, link[i].r_com);
	  temp9 = CrossProduct(link[i].omega_a, link[i].r_com);
	  temp10 = CrossProduct(link[i].omega, temp9);

	  link[i].Bframeddot=AddVector(link[i].Oframeddot, temp8, temp10); //LAR*_0

	  freesinglepointer(temp10);
	  freesinglepointer(temp9);
	  freesinglepointer(temp8);
	  freesinglepointer(temp24);
	  freesinglepointer(temp7);
	  freesinglepointer(temp6);
	  freesinglepointer(temp5);
	  freesinglepointer(temp23);
	  freesinglepointer(temp4);
	  freesinglepointer(temp3);
	  freesinglepointer(temp2);
	  freesinglepointer(temp22);
	  freesinglepointer(temp20);
	  freesinglepointer(temp21); 
	  freesinglepointer(temp1);
  }


   // Backward Iteration 

  //Initialization

  float fN_1[3]={{0},{0},{0}};
  float tauN_1[3]={{0},{0},{0}};

  
  // For Link 7

  temp11 = ScalarProduct(m[6], link[6].Bframeddot);
  temp25 = MatrixProduct2(RI, fN_1); //Transformation to proper reference frame (Refer Siavicco / Craig)

  link[6].force = AddVector(temp25, temp11); // FR

  temp12 = AddVector(link[6].r_end, link[6].r_com);
  temp13 = CrossProduct(link[6].force, temp12);
  temp14 = CrossProduct(temp25, link[6].r_com);
  temp15 = MatrixProduct2(link[6].I, link[6].omegadot);
  temp16 = MatrixProduct2(link[6].I, link[6].omega);
  temp17 = CrossProduct(link[6].omega_a, temp16);
  temp26 = MatrixProduct2(RI, tauN_1); //Transformation to proper reference frame (Refer Siavicco / Craig)
  temp18 = AddVector(temp26, temp14, temp15);
  temp19 = AddVector(temp18, temp17);
  
  link[6].tau = SubVector(temp19, temp13); // TR*_1

  u[6] = link[6].tau[0]*link[6].Rtrans[0][2] + link[6].tau[1]*link[6].Rtrans[1][2] + link[6].tau[2]*link[6].Rtrans[2][2]; //FP

  freesinglepointer(temp19);
  freesinglepointer(temp18);
  freesinglepointer(temp26);
  freesinglepointer(temp17);
  freesinglepointer(temp16);
  freesinglepointer(temp15);
  freesinglepointer(temp14);
  freesinglepointer(temp13);
  freesinglepointer(temp12);
  freesinglepointer(temp25);
  freesinglepointer(temp11);


 // For all other links

  for (int i=5;i>=0;i--)
  {
	  temp11 = newsinglepointer(temp11, 3);
	  temp12 = newsinglepointer(temp12, 3);
	  temp13 = newsinglepointer(temp13, 3);
	  temp14 = newsinglepointer(temp14, 3);
	  temp15 = newsinglepointer(temp15, 3);
	  temp16 = newsinglepointer(temp16, 3);
	  temp17 = newsinglepointer(temp17, 3);
	  temp18 = newsinglepointer(temp18, 3);
	  temp19 = newsinglepointer(temp19, 3);
	  temp25 = newsinglepointer(temp24, 3);
	  temp26 = newsinglepointer(temp25, 3);
	  
	  temp11 = ScalarProduct(m[i], link[i].Bframeddot);
	  temp25 = MatrixProduct2(link[i+1].R, link[i+1].force); //Transformation to proper reference frame (Refer Siavicco / Craig)

	  link[i].force = AddVector(temp25, temp11); // FR

      temp12 = AddVector(link[i].r_end, link[i].r_com);
	  temp13 = CrossProduct(link[i].force, temp12);
	  temp14 = CrossProduct(temp25, link[i].r_com);
	  temp15 = MatrixProduct2(link[i].I, link[i].omegadot);
	  temp16 = MatrixProduct2(link[i].I, link[i].omega);
	  temp17 = CrossProduct(link[i].omega_a, temp16);
	  temp26 = MatrixProduct2(link[i+1].R, link[i+1].tau); //Transformation to proper reference frame (Refer Siavicco / Craig)
	  temp18 = AddVector(temp26, temp14, temp15);
	  temp19 = AddVector(temp18, temp17);

	  link[i].tau = SubVector(temp19, temp13); // TR*_1

	  u[i] = link[i].tau[0]*link[i].Rtrans[0][2] + link[i].tau[1]*link[i].Rtrans[1][2] + link[i].tau[2]*link[i].Rtrans[2][2]; //FP

	  freesinglepointer(temp19);
	  freesinglepointer(temp18);
	  freesinglepointer(temp26);
	  freesinglepointer(temp17);
	  freesinglepointer(temp16);
	  freesinglepointer(temp15);
	  freesinglepointer(temp14);
	  freesinglepointer(temp13);
	  freesinglepointer(temp12);
	  freesinglepointer(temp25);
	  freesinglepointer(temp11);
  }

  freepointer(RI,3);

  for(int i=0;i<7;i++)
  {
	  freesinglepointer(link[i].force);
	  freesinglepointer(link[i].tau);
  }


  for(int i=6;i>=0;i--)
  {
	  freesinglepointer(link[i].Bframeddot);
	  freesinglepointer(link[i].Oframeddot);
	  freesinglepointer(link[i].omegadot);
	  freesinglepointer(link[i].omega_a);
	  freesinglepointer(link[i].omega);
      freepointer(link[i].Rtrans,3);
	  freepointer(link[i].R,3);
	  freepointer(link[i].I,3);
  }

   return u;

}

void InitializeDevices(rxSystem* sys, map<const string_type, rxDevice*>& devices)
{

	// Initialize Devices

	devices.clear();

	devices[_T("motor1_left")] = sys->findDevice(_T("motor1_left"));
	devices[_T("enc1_left")] = sys->findDevice(_T("enc1_left"));
	devices[_T("tach1_left")] = sys->findDevice(_T("tach1_left"));

	devices[_T("motor2_left")] = sys->findDevice(_T("motor2_left"));
	devices[_T("enc2_left")] = sys->findDevice(_T("enc2_left"));
	devices[_T("tach2_left")] = sys->findDevice(_T("tach2_left"));

	devices[_T("motor3_left")] = sys->findDevice(_T("motor3_left"));
	devices[_T("enc3_left")] = sys->findDevice(_T("enc3_left"));
	devices[_T("tach3_left")] = sys->findDevice(_T("tach3_left"));

	devices[_T("motor4_left")] = sys->findDevice(_T("motor4_left"));
	devices[_T("enc4_left")] = sys->findDevice(_T("enc4_left"));
	devices[_T("tach4_left")] = sys->findDevice(_T("tach4_left"));

	devices[_T("motor5_left")] = sys->findDevice(_T("motor5_left"));
	devices[_T("enc5_left")] = sys->findDevice(_T("enc5_left"));
	devices[_T("tach5_left")] = sys->findDevice(_T("tach5_left"));

	devices[_T("motor6_left")] = sys->findDevice(_T("motor6_left"));
	devices[_T("enc6_left")] = sys->findDevice(_T("enc6_left"));
	devices[_T("tach6_left")] = sys->findDevice(_T("tach6_left"));

	devices[_T("motor7_left")] = sys->findDevice(_T("motor7_left"));
	devices[_T("enc7_left")] = sys->findDevice(_T("enc7_left"));
	devices[_T("tach7_left")] = sys->findDevice(_T("tach7_left"));

}

void UpdateControl(rTime time, map<const string_type, rxDevice*>& devices)
{
	// Read Encoder, Tachometer, and Accelerometer Values

	devices[_T("enc1_left")]->readDeviceValue(&q[0], 1*sizeof(float));
	devices[_T("tach1_left")]->readDeviceValue(&dq[0], 1*sizeof(float));

	devices[_T("enc2_left")]->readDeviceValue(&q[1], 1*sizeof(float));
	devices[_T("tach2_left")]->readDeviceValue(&dq[1], 1*sizeof(float));

	devices[_T("enc3_left")]->readDeviceValue(&q[2], 1*sizeof(float));
	devices[_T("tach3_left")]->readDeviceValue(&dq[2], 1*sizeof(float));

	devices[_T("enc4_left")]->readDeviceValue(&q[3], 1*sizeof(float));
	devices[_T("tach4_left")]->readDeviceValue(&dq[3], 1*sizeof(float));

	devices[_T("enc5_left")]->readDeviceValue(&q[4], 1*sizeof(float));
	devices[_T("tach5_left")]->readDeviceValue(&dq[4], 1*sizeof(float));

	devices[_T("enc6_left")]->readDeviceValue(&q[5], 1*sizeof(float));
	devices[_T("tach6_left")]->readDeviceValue(&dq[5], 1*sizeof(float));

	devices[_T("enc7_left")]->readDeviceValue(&q[6], 1*sizeof(float));
	devices[_T("tach7_left")]->readDeviceValue(&dq[6], 1*sizeof(float));

	
	// Calculating transformation matrix

	float s1 = sin(q[0]);
	float c1 = cos(q[0]);

	float s2 = sin(q[1]);
	float c2 = cos(q[1]);

	float s3 = sin(q[2]);
	float c3 = cos(q[2]);

	float s4 = sin(q[3]);
	float c4 = cos(q[3]);

	float s5 = sin(q[4]);
	float c5 = cos(q[4]);

	float s6 = sin(q[5]);
	float c6 = cos(q[5]);

	float s7 = sin(q[6]);
	float c7 = cos(q[6]);

	float Array1[4][4] = { {-c1, 0, -s1, 0},
							{-s1, 0, c1, 0},
							{0, 1, 0, l1},
							{0, 0, 0, 1} };

	float Array2[4][4] = { {-s2, 0, c2, 0},
							{c2, 0, s2, 0},
							{0, 1, 0, 0},
							{0, 0, 0, 1} };

	float Array3[4][4] = { {-s3, 0, c3, 0},
							{c3, 0, s3, 0},
							{0, 1, 0, (l2+l3)},
							{0, 0, 0, 1} };

	float Array4[4][4] = { {-c4, 0, -s4, 0},
							{-s4, 0, c4, 0},
							{0, 1, 0, 0},
							{0, 0, 0, 1} };

	float Array5[4][4] = { {-c5, 0, -s5, 0},
							{-s5, 0, c5, 0},
							{0, 1, 0, (l4+l5)},
							{0, 0, 0, 1} };

	float Array6[4][4] = { {-s6, 0, c6, -(l6*s6)},
							{c6, 0, s6, (l6*c6)},
							{0, 1, 0, 0},
							{0, 0, 0, 1} };

	float Array7[4][4] = { {c7, -s7, 0, (l7*c7)},
							{s7, c7, 0, (l7*s7)},
							{0, 0, 1, 0},
							{0, 0, 0, 1} };

	A1 = newpointer(A1,4,4);
	A2 = newpointer(A2,4,4);
	A3 = newpointer(A3,4,4);
	A4 = newpointer(A4,4,4);
	A5 = newpointer(A5,4,4);
	A6 = newpointer(A6,4,4);
	A7 = newpointer(A7,4,4);

	T1 = newpointer(T1,4,4);
	T2 = newpointer(T2,4,4);
	T3 = newpointer(T3,4,4);
	T4 = newpointer(T4,4,4);
	T5 = newpointer(T5,4,4);
	T6 = newpointer(T6,4,4);
	T7 = newpointer(T7,4,4);

	assign(A1, Array1);
	assign(A2, Array2);
	assign(A3, Array3);
	assign(A4, Array4);
	assign(A5, Array5);
	assign(A6, Array6);
	assign(A7, Array7);

	T1 = A1;
	T2 = MatrixProduct(A1, A2);
	T3 = MatrixProduct(T2, A3);
	T4 = MatrixProduct(T3, A4);
	T5 = MatrixProduct(T4, A5);
	T6 = MatrixProduct(T5, A6);
	T7 = MatrixProduct(T6, A7);

	// Print Transformation Matrices of arm

   /*
   print(T1);
   printf("\n");
   print(T2);
   printf("\n");
   print(T3);
   printf("\n");
   print(T4);
   printf("\n");
   print(T5);
   printf("\n");
   print(T6);
   printf("\n");
   print(T7);
   fprintf(f,"\n");*/

	float Z0[3] = {0, 0, 1};
	float U0[3] = {0, 0, 0};

	float Z1[3] = {T1[0][2], T1[1][2], T1[2][2]};
	float U1[3] = {T1[0][3], T1[1][3], T1[2][3]};

	float Z2[3] = {T2[0][2], T2[1][2], T2[2][2]};
	float U2[3] = {T2[0][3], T2[1][3], T2[2][3]};

	float Z3[3] = {T3[0][2], T3[1][2], T3[2][2]};
	float U3[3] = {T3[0][3], T3[1][3], T3[2][3]};

	float Z4[3] = {T4[0][2], T4[1][2], T4[2][2]};
	float U4[3] = {T4[0][3], T4[1][3], T4[2][3]};

	float Z5[3] = {T5[0][2], T5[1][2], T5[2][2]};
	float U5[3] = {T5[0][3], T5[1][3], T5[2][3]};

	float Z6[3] = {T6[0][2], T6[1][2], T6[2][2]};
	float U6[3] = {T6[0][3], T6[1][3], T6[2][3]};


	float Z7[3] = {T7[0][2], T7[1][2], T7[2][2]};
	float U7[3] = {T7[0][3], T7[1][3], T7[2][3]};


	J1_upperhalf = newsinglepointer(J1_upperhalf, 3);
	J2_upperhalf = newsinglepointer(J2_upperhalf, 3);
	J3_upperhalf = newsinglepointer(J3_upperhalf, 3);
	J4_upperhalf = newsinglepointer(J4_upperhalf, 3);
	J5_upperhalf = newsinglepointer(J5_upperhalf, 3);
	J6_upperhalf = newsinglepointer(J6_upperhalf, 3);
	J7_upperhalf = newsinglepointer(J7_upperhalf, 3);

	float U7_0[3]={0}, U7_1[3]={0}, U7_2[3]={0}, U7_3[3]={0}, U7_4[3]={0}, U7_5[3]={0}, U7_6[3]={0}; 

	for (int i=0; i<3; i++)
	{
		U7_0[i] = U7[i] - U0[i];
		U7_1[i] = U7[i] - U1[i];
		U7_2[i] = U7[i] - U2[i];
		U7_3[i] = U7[i] - U3[i];
		U7_4[i] = U7[i] - U4[i];
		U7_5[i] = U7[i] - U5[i];
		U7_6[i] = U7[i] - U6[i];
	}

	J1_upperhalf = CrossProduct(Z0, U7_0);
	J2_upperhalf = CrossProduct(Z1, U7_1);
	J3_upperhalf = CrossProduct(Z2, U7_2);
	J4_upperhalf = CrossProduct(Z3, U7_3);
	J5_upperhalf = CrossProduct(Z4, U7_4);
	J6_upperhalf = CrossProduct(Z5, U7_5);
	J7_upperhalf = CrossProduct(Z6, U7_6);

	//Formation of Jacobian....

	J = newpointer(J,6,7);

	int j=0;

	for (int i=0; i<3; i++)
	{
		J[i][j] = J1_upperhalf[i];
	}
	for (int i=3; i<6; i++)
	{
		J[i][j] = Z0[i-3];
	}

	j=1;

	for (int i=0; i<3; i++)
	{
		J[i][j] = J2_upperhalf[i];
	}
	for (int i=3; i<6; i++)
	{
		J[i][j] = Z1[i-3];
	}

	j=2;

	for (int i=0; i<3; i++)
	{
		J[i][j] = J3_upperhalf[i];
	}
	for (int i=3; i<6; i++)
	{
		J[i][j] = Z2[i-3];
	}

	j=3;

	for (int i=0; i<3; i++)
	{
		J[i][j] = J4_upperhalf[i];
	}
	for (int i=3; i<6; i++)
	{
		J[i][j] = Z3[i-3];
	}

	j=4;

	for (int i=0; i<3; i++)
	{
		J[i][j] = J5_upperhalf[i];
	}
	for (int i=3; i<6; i++)
	{
		J[i][j] = Z4[i-3];
	}

	j=5;

	for (int i=0; i<3; i++)
	{
		J[i][j] = J6_upperhalf[i];
	}
	for (int i=3; i<6; i++)
	{
		J[i][j] = Z5[i-3];
	}

	j=6;

	for (int i=0; i<3; i++)
	{
		J[i][j] = J7_upperhalf[i];
	}
	for (int i=3; i<6; i++)
	{
		J[i][j] = Z6[i-3];
	}

	// Print Jacobian of Arm

	
/*
	for (int i=0; i<6; i++)
	{
		for(int j=0; j<7;j++)
		{
			fprintf(f,"%.2f\t", J[i][j]);
		}
		fprintf(f,"\n");
	}
	//getch();
	fprintf(f,"\n");
*/
	

    //Formation of Jacobian transpose

	Jtrans = newpointer(Jtrans,7,6);
	
	for (int i=0; i<6; i++)
	{
		for (int j=0; j<7; j++)
		{
			Jtrans[j][i] = J[i][j];
		}
	}

	// Print Jacobian Arm Transpose

    /*
	for (int i=0; i<7; i++)
	{
		for(int j=0; j<6;j++)
		{
			printf("%.2f\t", Jtrans[i][j]);
		}
		printf("\n");
	}
	//getch();
	printf("\n");
	*/

	// Calculate time-derivative of Jacobian

	Z0dot = newsinglepointer(Z0dot, 3);
	Z1dot = newsinglepointer(Z1dot, 3);
	Z2dot = newsinglepointer(Z2dot, 3);
	Z3dot = newsinglepointer(Z3dot, 3);
	Z4dot = newsinglepointer(Z4dot, 3);
	Z5dot = newsinglepointer(Z5dot, 3);
	Z6dot = newsinglepointer(Z6dot, 3);

	Udot1_0 = newsinglepointer(Udot1_0, 3);
	Udot2_1 = newsinglepointer(Udot2_1, 3);
	Udot3_2 = newsinglepointer(Udot3_2, 3);
	Udot4_3 = newsinglepointer(Udot4_3, 3);
	Udot5_4 = newsinglepointer(Udot5_4, 3);
	Udot6_5 = newsinglepointer(Udot6_5, 3);
	Udot7_6 = newsinglepointer(Udot7_6, 3);

	J1dot_upperhalf = newsinglepointer(J1dot_upperhalf, 3);
	J2dot_upperhalf = newsinglepointer(J2dot_upperhalf, 3);
	J3dot_upperhalf = newsinglepointer(J3dot_upperhalf, 3);
	J4dot_upperhalf = newsinglepointer(J4dot_upperhalf, 3);
	J5dot_upperhalf = newsinglepointer(J5dot_upperhalf, 3);
	J6dot_upperhalf = newsinglepointer(J6dot_upperhalf, 3);
	J7dot_upperhalf = newsinglepointer(J7dot_upperhalf, 3);

	omega_0 = newsinglepointer(omega_0, 3);
	omega_1 = newsinglepointer(omega_1, 3);
	omega_2 = newsinglepointer(omega_2, 3);
	omega_3 = newsinglepointer(omega_3, 3);
	omega_4 = newsinglepointer(omega_4, 3);
	omega_5 = newsinglepointer(omega_5, 3);
	omega_6 = newsinglepointer(omega_6, 3);

	Jdottemp1 = newsinglepointer(Jdottemp1,3);
	Jdottemp2 = newsinglepointer(Jdottemp2,3);
	Jdottemp3 = newsinglepointer(Jdottemp3,3);
	Jdottemp4 = newsinglepointer(Jdottemp4,3);
	Jdottemp5 = newsinglepointer(Jdottemp5,3);
    Jdottemp6 = newsinglepointer(Jdottemp6,3);
	
	omega_0 = ScalarProduct(dq[0],Z0);
	
	Jdottemp1 = ScalarProduct(dq[1],Z1);
	omega_1 = AddVector(omega_0,Jdottemp1);

	Jdottemp2 = ScalarProduct(dq[2],Z2);
	omega_2 = AddVector(omega_1,Jdottemp2);

	Jdottemp3 = ScalarProduct(dq[3],Z3);
	omega_3 = AddVector(omega_2,Jdottemp3);

	Jdottemp4 = ScalarProduct(dq[4],Z4);
	omega_4 = AddVector(omega_3,Jdottemp4);

	Jdottemp5 = ScalarProduct(dq[5],Z5);
	omega_5 = AddVector(omega_4,Jdottemp5);

	Jdottemp6 = ScalarProduct(dq[6],Z6);
	omega_6 = AddVector(omega_5,Jdottemp6);


	Z0dot = CrossProduct(omega_0, Z0);
    Z1dot = CrossProduct(omega_1, Z1);
	Z2dot = CrossProduct(omega_2, Z2);
	Z3dot = CrossProduct(omega_3, Z3);
	Z4dot = CrossProduct(omega_4, Z4);
	Z5dot = CrossProduct(omega_5, Z5);
	Z6dot = CrossProduct(omega_6, Z6);

	float U1_0[3]={0}, U2_1[3]={0}, U3_2[3]={0}, U4_3[3]={0}, U5_4[3]={0}, U6_5[3]={0}; 

	for (int i=0; i<3; i++)
	{
		U1_0[i] = U1[i] - U0[i];
		U2_1[i] = U2[i] - U1[i];
		U3_2[i] = U3[i] - U2[i];
		U4_3[i] = U4[i] - U3[i];
		U5_4[i] = U5[i] - U4[i];
		U6_5[i] = U6[i] - U5[i];
	}

	Udot1_0 = CrossProduct(omega_0, U1_0);
	Udot2_1 = CrossProduct(omega_1, U2_1);
	Udot3_2 = CrossProduct(omega_2, U3_2);
	Udot4_3 = CrossProduct(omega_3, U4_3);
	Udot5_4 = CrossProduct(omega_4, U5_4);
	Udot6_5 = CrossProduct(omega_5, U6_5);
	Udot7_6 = CrossProduct(omega_6, U7_6);

	float Udot7_0[3] = {{Udot7_6[0]+Udot6_5[0]+Udot5_4[0]+Udot4_3[0]+Udot3_2[0]+Udot2_1[0]+Udot1_0[0]},{Udot7_6[1]+Udot6_5[1]+Udot5_4[1]+Udot4_3[1]+Udot3_2[1]+Udot2_1[1]+Udot1_0[1]},{Udot7_6[2]+Udot6_5[2]+Udot5_4[2]+Udot4_3[2]+Udot3_2[2]+Udot2_1[2]+Udot1_0[2]}};
	float Udot7_1[3] = {{Udot7_6[0]+Udot6_5[0]+Udot5_4[0]+Udot4_3[0]+Udot3_2[0]+Udot2_1[0]},{Udot7_6[1]+Udot6_5[1]+Udot5_4[1]+Udot4_3[1]+Udot3_2[1]+Udot2_1[1]},{Udot7_6[2]+Udot6_5[2]+Udot5_4[2]+Udot4_3[2]+Udot3_2[2]+Udot2_1[2]}};
	float Udot7_2[3] = {{Udot7_6[0]+Udot6_5[0]+Udot5_4[0]+Udot4_3[0]+Udot3_2[0]},{Udot7_6[1]+Udot6_5[1]+Udot5_4[1]+Udot4_3[1]+Udot3_2[1]},{Udot7_6[2]+Udot6_5[2]+Udot5_4[2]+Udot4_3[2]+Udot3_2[2]}};
	float Udot7_3[3] = {{Udot7_6[0]+Udot6_5[0]+Udot5_4[0]+Udot4_3[0]},{Udot7_6[1]+Udot6_5[1]+Udot5_4[1]+Udot4_3[1]},{Udot7_6[2]+Udot6_5[2]+Udot5_4[2]+Udot4_3[2]}};
	float Udot7_4[3] = {{Udot7_6[0]+Udot6_5[0]+Udot5_4[0]},{Udot7_6[1]+Udot6_5[1]+Udot5_4[1]},{Udot7_6[2]+Udot6_5[2]+Udot5_4[2]}};
	float Udot7_5[3] = {{Udot7_6[0]+Udot6_5[0]},{Udot7_6[1]+Udot6_5[1]},{Udot7_6[2]+Udot6_5[2]}};


	term1 = newsinglepointer(term1, 3);
	term2 = newsinglepointer(term2, 3);
	term3 = newsinglepointer(term3, 3);
	term4 = newsinglepointer(term4, 3);
	term5 = newsinglepointer(term5, 3);
	term6 = newsinglepointer(term6, 3);
	term7 = newsinglepointer(term7, 3);
	term8 = newsinglepointer(term8, 3);
	term9 = newsinglepointer(term9, 3);
	term10 = newsinglepointer(term10, 3);
	term11 = newsinglepointer(term11, 3);
	term12 = newsinglepointer(term12, 3);
	term13 = newsinglepointer(term13, 3);
	term14 = newsinglepointer(term14, 3);

	term1 = CrossProduct(Z0dot, U7_0);
    term2 = CrossProduct(Z1dot, U7_1);
	term3 = CrossProduct(Z2dot, U7_2);
	term4 = CrossProduct(Z3dot, U7_3);
	term5 = CrossProduct(Z4dot, U7_4);
	term6 = CrossProduct(Z5dot, U7_5);
	term7 = CrossProduct(Z6dot, U7_6);

	term8 = CrossProduct(Z0, Udot7_0);
	term9 = CrossProduct(Z1, Udot7_1);
	term10 = CrossProduct(Z2, Udot7_2);
	term11 = CrossProduct(Z3, Udot7_3);
	term12 = CrossProduct(Z4, Udot7_4);
	term13 = CrossProduct(Z5, Udot7_5);
	term14 = CrossProduct(Z6, Udot7_6);

	J1dot_upperhalf = AddVector(term1, term8);
    J2dot_upperhalf = AddVector(term2, term9);
	J3dot_upperhalf = AddVector(term3, term10);
	J4dot_upperhalf = AddVector(term4, term11);
	J5dot_upperhalf = AddVector(term5, term12);
	J6dot_upperhalf = AddVector(term6, term13);
	J7dot_upperhalf = AddVector(term7, term14);

	// Formation of the Jdot matrix

	Jdot = newpointer(Jdot,6,7);

	j=0;

	for (int i=0; i<3; i++)
	{
		Jdot[i][j] = J1dot_upperhalf[i];
	}
	for (int i=3; i<6; i++)
	{
		Jdot[i][j] = Z0dot[i-3];
	}

	j=1;

	for (int i=0; i<3; i++)
	{
		Jdot[i][j] = J2dot_upperhalf[i];
	}
	for (int i=3; i<6; i++)
	{
		Jdot[i][j] = Z1dot[i-3];
	}

	j=2;

	for (int i=0; i<3; i++)
	{
		Jdot[i][j] = J3dot_upperhalf[i];
	}
	for (int i=3; i<6; i++)
	{
		Jdot[i][j] = Z2dot[i-3];
	}

	j=3;

	for (int i=0; i<3; i++)
	{
		Jdot[i][j] = J4dot_upperhalf[i];
	}
	for (int i=3; i<6; i++)
	{
		Jdot[i][j] = Z3dot[i-3];
	}

	j=4;

	for (int i=0; i<3; i++)
	{
		Jdot[i][j] = J5dot_upperhalf[i];
	}
	for (int i=3; i<6; i++)
	{
		Jdot[i][j] = Z4dot[i-3];
	}

	j=5;

	for (int i=0; i<3; i++)
	{
		Jdot[i][j] = J6dot_upperhalf[i];
	}
	for (int i=3; i<6; i++)
	{
		Jdot[i][j] = Z5dot[i-3];
	}

	j=6;

	for (int i=0; i<3; i++)
	{
		Jdot[i][j] = J7dot_upperhalf[i];
	}
	for (int i=3; i<6; i++)
	{
		Jdot[i][j] = Z6dot[i-3];
	}

    // Print Jdot of Arm

/*
    fprintf(f,"Jdot Matrix\n");
	for (int i=0; i<6; i++)
	{
		for(int j=0; j<7;j++)
		{
			fprintf(f,"%.4f\t", Jdot[i][j]);
		}
		fprintf(f,"\n");
	}
	//getch();
	fprintf(f,"\n");
*/
	

	// Identifying Parameters using Modified Newton-Euler Inverse Dynamics Algorithm

	// Joint-Space Inertia Matrix


	H1 = newsinglepointer(H1,7);
	H2 = newsinglepointer(H2,7);
	H3 = newsinglepointer(H3,7);
	H4 = newsinglepointer(H4,7);
	H5 = newsinglepointer(H5,7);
	H6 = newsinglepointer(H6,7);
	H7 = newsinglepointer(H7,7);

	H = newpointer(H,7,7);

	H1 = inverse_dyn(q, null_v, null_v, V1);

	j=0;

	for (int i=0; i<7; i++)
	{
		H[i][j] = H1[i];
	}


	H2 = inverse_dyn(q, null_v, null_v, V2);

	j=1;

	for (int i=0; i<7; i++)
	{
		H[i][j] = H2[i];
	}


	H3 = inverse_dyn(q, null_v, null_v, V3);

	j=2;

	for (int i=0; i<7; i++)
	{
		H[i][j] = H3[i];
	}

	H4 = inverse_dyn(q, null_v, null_v, V4);

	j=3;

	for (int i=0; i<7; i++)
	{
		H[i][j] = H4[i];
	}

	H5 = inverse_dyn(q, null_v, null_v, V5);

	j=4;

	for (int i=0; i<7; i++)
	{
		H[i][j] = H5[i];
	}

	H6 = inverse_dyn(q, null_v, null_v, V6);

	j=5;

	for (int i=0; i<7; i++)
	{
		H[i][j] = H6[i];
	}

	H7 = inverse_dyn(q, null_v, null_v, V7);

	
	j=6;

	for (int i=0; i<7; i++)
	{
		H[i][j] = H7[i];
	}

/*
    fprintf(f,"H Matrix\n");
	for (int i=0; i<7; i++)
	{
		for(int j=0; j<7;j++)
		{
			fprintf(f,"%.6f\t", H[i][j]);
		}
		fprintf(f,"\n");
	}
	//getch();
	fprintf(f,"\n");
*/

	// Joint-Space Coupling Matrix (Centrifugal and Coriolis terms)

	C1 = newsinglepointer(C1,7);
	C2 = newsinglepointer(C2,7);
	C3 = newsinglepointer(C3,7);
	C4 = newsinglepointer(C4,7);
	C5 = newsinglepointer(C5,7);
	C6 = newsinglepointer(C6,7);
	C7 = newsinglepointer(C7,7);

	C = newpointer(C,7,7);

	C1 = inverse_dyn(q, dq, V1, null_v);

	j=0;

	for (int i=0; i<7; i++)
	{
		C[i][j] = C1[i];
	}

	C2 = inverse_dyn(q, dq, V2, null_v);

	j=1;

	for (int i=0; i<7; i++)
	{
		C[i][j] = C2[i];
	}

	C3 = inverse_dyn(q, dq, V3, null_v);

	j=2;

	for (int i=0; i<7; i++)
	{
		C[i][j] = C3[i];
	}

	C4 = inverse_dyn(q, dq, V4, null_v);

	j=3;

	for (int i=0; i<7; i++)
	{
		C[i][j] = C4[i];
	}

	C5 = inverse_dyn(q, dq, V5, null_v);

	j=4;

	for (int i=0; i<7; i++)
	{
		C[i][j] = C5[i];
	}

	C6 = inverse_dyn(q, dq, V6, null_v);

	j=5;

	for (int i=0; i<7; i++)
	{
		C[i][j] = C6[i];
	}

	C7 = inverse_dyn(q, dq, V7, null_v);

	j=6;

	for (int i=0; i<7; i++)
	{
		C[i][j] = C7[i];
	}
/*
    fprintf(f,"C Matrix\n");
	for (int i=0; i<7; i++)
	{
		for(int j=0; j<7;j++)
		{
			fprintf(f,"%.6f\t", C[i][j]);
		}
		fprintf(f,"\n");
	}
	//getch();
	fprintf(f,"\n");
*/
		
	// Task-space Inertia Matrix

	J_Jtrans = newpointer(J_Jtrans,6,6);
	inertemp1 = newpointer(inertemp1,6,6);
	J_inv = newpointer(J_inv,7,6);
	Jtrans_inv = newpointer(Jtrans_inv,6,7);
	inertemp2 = newpointer(inertemp2,6,7);
	H_task = newpointer(H_task,6,6);

	//MoorePenrose PseudoInverse for full row-rank Jacobian matrix

	// Test
	
   J_Jtrans = MatrixProduct3(J,Jtrans,6,7,7,6);

/*
	for (int i=0; i<6; i++)
	{
		for(int j=0; j<6;j++)
		{
			fprintf(f,"%.6f\t", J_Jtrans[i][j]);
		}
		fprintf(f,"\n");
	}
	//getch();
	fprintf(f,"\n");*/

	inertemp1 = inverse_mat(J_Jtrans,6);

/*
	for (int i=0; i<6; i++)
	{
		for(int j=0; j<6;j++)
		{
			fprintf(f,"%.6f\t", inertemp1[i][j]);
		}
		fprintf(f,"\n");
	}
	//getch();
	fprintf(f,"\n");*/


	J_inv = MatrixProduct3(Jtrans,inertemp1,7,6,6,6);

	//Formation of J_inv transpose

	for (int i=0; i<7; i++)
	{
		for (int j=0; j<6; j++)
		{
			Jtrans_inv[j][i] = J_inv[i][j];
		}
	}

	inertemp2 = MatrixProduct3(Jtrans_inv,H,6,7,7,7);
	
	H_task = MatrixProduct3(inertemp2,J_inv,6,7,7,6);
/*
	for (int i=0; i<6; i++)
	{
		for(int j=0; j<6;j++)
		{
			fprintf(f, "%.6f\t", H_task[i][j]);
		}
		fprintf(f, "\n");
	}
	//getch();
	fprintf(f, "\n");
*/


	// Task-space Coupling Matrix (Centrifugal and Coriolis terms)

	cortemp1 = newpointer(cortemp1,6,7);
	cortemp2 = newpointer(cortemp2,6,6);
	cortemp3 = newpointer(cortemp3,6,7);
	cortemp4 = newpointer(cortemp3,6,6);
	C_task = newpointer(C_task,6,6);


	cortemp1 = MatrixProduct3(Jtrans_inv,C,6,7,7,7);
	cortemp2 = MatrixProduct3(cortemp1,J_inv,6,7,7,6);
	cortemp3 = MatrixProduct3(H_task,Jdot,6,6,6,7);
	cortemp4 = MatrixProduct3(cortemp3,J_inv,6,7,7,6);

	C_task = SubMatrix(cortemp2,cortemp4);

/*
	for (int i=0; i<6; i++)
	{
		for(int j=0; j<6;j++)
		{
			fprintf(f, "%.6f\t", C_task[i][j]);
		}
		fprintf(f, "\n");
	}
	//getch();
	fprintf(f, "\n");
*/


  	// Calculating end-effector position

	X = T7[0][3]; 
	Y = T7[1][3]; 
	Z = T7[2][3]; 

	//fprintf(f,"%f\t\t %f\t\t %f\t\t %f\t\t %f\t\t %f\t\t %f\t\t %f\t\t %f\t\t %f\t\t %f\n", t, q[0], q[1], q[2], q[3], q[4], q[5], q[6], X, Y, Z);

	// Calculating end-effector velocity

	Xvel = J[0][0]*dq[0] + J[0][1]*dq[1] + J[0][2]*dq[2] + J[0][3]*dq[3] + J[0][4]*dq[4] + J[0][5]*dq[5] + J[0][6]*dq[6];
    Yvel = J[1][0]*dq[0] + J[1][1]*dq[1] + J[1][2]*dq[2] + J[1][3]*dq[3] + J[1][4]*dq[4] + J[1][5]*dq[5] + J[1][6]*dq[6];
	Zvel = J[2][0]*dq[0] + J[2][1]*dq[1] + J[2][2]*dq[2] + J[2][3]*dq[3] + J[2][4]*dq[4] + J[2][5]*dq[5] + J[2][6]*dq[6];
	Xangvel = J[3][0]*dq[0] + J[3][1]*dq[1] + J[3][2]*dq[2] + J[3][3]*dq[3] + J[3][4]*dq[4] + J[3][5]*dq[5] + J[3][6]*dq[6];
	Yangvel = J[4][0]*dq[0] + J[4][1]*dq[1] + J[4][2]*dq[2] + J[4][3]*dq[3] + J[4][4]*dq[4] + J[4][5]*dq[5] + J[4][6]*dq[6];
	Zangvel = J[5][0]*dq[0] + J[5][1]*dq[1] + J[5][2]*dq[2] + J[5][3]*dq[3] + J[5][4]*dq[4] + J[5][5]*dq[5] + J[5][6]*dq[6];

	//fprintf(f,"Xvel = %f, Yvel = %f, Zvel = %f, Xangvel = %f, Yangvel = %f, Zangvel = %f\n", Xvel, Yvel, Zvel, Xangvel, Yangvel, Zangvel); 

    float end_vel = sqrt((Xvel*Xvel) + (Yvel*Yvel) + (Zvel*Zvel));

	fprintf(f,"%f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\t %f\n", X, Z, t, q[0], t, q[1], t, q[2], t, q[3], t, q[4], t, q[5], t, q[6]); 
    
	// Computing input force

	float posdelnorm = sqrt((X-Xd)*(X-Xd)+ (Y-Yd)*(Y-Yd) + (Z-Zd)*(Z-Zd));
	
	 // Assigning damping matrix

	float C1param = 0.01;
	float C2param = 0.5;
	float C3param = 0.01;
	float C4param = 0.5;
	float C5param = 0.01;
    float C6param = 0.01;
	float C7param = 0.5;

	float Cdash[7] = {0};

   Cdash[0] = C1param*sin(3.1416*(posdel0norm - posdelnorm)/(2*posdel0norm));
   Cdash[1] = C2param*sin(3.1416*(posdel0norm - posdelnorm)/(2*posdel0norm));
   Cdash[2] = C3param*sin(3.1416*(posdel0norm - posdelnorm)/(2*posdel0norm));
   Cdash[3] = C4param*sin(3.1416*(posdel0norm - posdelnorm)/(2*posdel0norm));
   Cdash[4] = C5param*sin(3.1416*(posdel0norm - posdelnorm)/(2*posdel0norm));
   Cdash[5] = C6param*sin(3.1416*(posdel0norm - posdelnorm)/(2*posdel0norm));
   Cdash[6] = C7param*sin(3.1416*(posdel0norm - posdelnorm)/(2*posdel0norm));

   //  Assigning posdel matrix

   float posdel[3] = {0};

   posdel[0] = X-Xd;
   posdel[1] = Y-Yd;
   posdel[2] = Z-Zd;

   //  Assigning Fmus matrix

   float f1 = 0.01;
   float f2 = 5;
   float f3 = 0.01;
   float f4 = 5;
   float f5 = 0.01;
   float f6 = 0.01;
   float f7 = 0.5;

   float Fmus[7] = {0};

   Fmus[0] = f1*cos(3.1416*(posdel0norm - posdelnorm)/(2*posdel0norm));
   Fmus[1] = f2*cos(3.1416*(posdel0norm - posdelnorm)/(2*posdel0norm));
   Fmus[2] = f3*cos(3.1416*(posdel0norm - posdelnorm)/(2*posdel0norm));
   Fmus[3] = f4*cos(3.1416*(posdel0norm - posdelnorm)/(2*posdel0norm));
   Fmus[4] = f5*cos(3.1416*(posdel0norm - posdelnorm)/(2*posdel0norm));
   Fmus[5] = f6*cos(3.1416*(posdel0norm - posdelnorm)/(2*posdel0norm));
   Fmus[6] = f7*cos(3.1416*(posdel0norm - posdelnorm)/(2*posdel0norm));

   // Calculate task-space force

   	input_force[0] = 0;
	input_force[1] = 0;
    input_force[2] = 0;

	k = 4;

	input_force[0] = k*(Xd-X);
	input_force[1] = k*(Yd-Y);
    input_force[2] = k*(Zd-Z);

/*
	Xd = X;
	Yd = Y;
	Zd = Z;	
	
	Kp_x = 4;
    Kp_y = 4;
	Kp_z = 4;
	Kd_x = 0.001;
	Kd_y = 0.001;
	Kd_z = 0.001;

	//input_force[0] = (1/T)*(Kp_x*T*(Xd-X) + Kp_x*T*(Xd-X_1T) + 2*Kd_x*(Xd-X) - 2*Kd_x*(Xd-X_1T) - T*input_force_1T[0]);
	//input_force[1] = (1/T)*(Kp_y*T*(Yd-Y) + Kp_y*T*(Yd-Y_1T) + 2*Kd_y*(Yd-Y) - 2*Kd_y*(Yd-Y_1T) - T*input_force_1T[1]);
    //input_force[2] = (1/T)*(Kp_z*T*(Zd-Z) + Kp_z*T*(Zd-Z_1T) + 2*Kd_z*(Zd-Z) - 2*Kd_z*(Zd-Z_1T) - T*input_force_1T[2]);

	//input_force[0] = Kp_x*(Xd-X) + Kd_x*Xvel;
	//input_force[1] = Kp_x*(Yd-Y) + Kd_x*Yvel;
	//input_force[2] = Kp_x*(Zd-Z) + Kd_x*Zvel;

	input_force[0] = Kp_x*(Xd-X);
	input_force[1] = Kp_y*(Yd-Y);
	input_force[2] = Kp_z*(Zd-Z);
*/

	//fprintf(f,"%f\t\t %f\t\t %f\n", input_force[0], input_force[1], input_force[2]);

	//Task space Disturbance Observers (3rd-order Q-filter)

	// X-Direction

	float Tau_x = 1/(2*3.14*10);  // Time-Constant of Q-Filter in X-Axis

	float coeff_1 = 12*T*pow(Tau_x,2) + 8*pow(Tau_x,3);
    float coeff_2 = -12*T*pow(Tau_x,2) - 24*pow(Tau_x,3);
	float coeff_3 = -12*T*pow(Tau_x,2) + 24*pow(Tau_x,3);
	float coeff_4 = 12*T*pow(Tau_x,2) - 8*pow(Tau_x,3);

	float coeff_5 = pow(T,3) + 6*pow(T,2)*Tau_x + 12*T*pow(Tau_x,2) + 8*pow(Tau_x,3);
	float coeff_6 = 3*pow(T,3) + 6*pow(T,2)*Tau_x - 12*T*pow(Tau_x,2) - 24*pow(Tau_x,3);
	float coeff_7 = 3*pow(T,3) - 6*pow(T,2)*Tau_x - 12*T*pow(Tau_x,2) + 24*pow(Tau_x,3);
	float coeff_8 = pow(T,3) - 6*pow(T,2)*Tau_x + 12*T*pow(Tau_x,2) - 8*pow(Tau_x,3);

	//fprintf(f,"coeff_8 = %f\n", coeff_8); 
/*	
	// Including Orientation

	float M1=H_task[0][0]*Xvel + H_task[0][1]*Yvel + H_task[0][2]*Zvel + H_task[0][3]*Xangvel + H_task[0][4]*Yangvel + H_task[0][5]*Zangvel;
	float B1=C_task[0][0]*Xvel + C_task[0][1]*Yvel + C_task[0][2]*Zvel + C_task[0][3]*Xangvel + C_task[0][4]*Yangvel + C_task[0][5]*Zangvel;

	//fprintf(f,"M1 = %f\t\t B1 = %f\n", M1, B1); 

	float M1_1T=H_task[0][0]*Xvel_1T + H_task[0][1]*Yvel_1T + H_task[0][2]*Zvel_1T + H_task[0][3]*Xangvel_1T + H_task[0][4]*Yangvel_1T + H_task[0][5]*Zangvel_1T;
	float B1_1T=C_task[0][0]*Xvel_1T + C_task[0][1]*Yvel_1T + C_task[0][2]*Zvel_1T + C_task[0][3]*Xangvel_1T + C_task[0][4]*Yangvel_1T + C_task[0][5]*Zangvel_1T;

	float M1_2T=H_task[0][0]*Xvel_2T + H_task[0][1]*Yvel_2T + H_task[0][2]*Zvel_2T + H_task[0][3]*Xangvel_2T + H_task[0][4]*Yangvel_2T + H_task[0][5]*Zangvel_2T;
	float B1_2T=C_task[0][0]*Xvel_2T + C_task[0][1]*Yvel_2T + C_task[0][2]*Zvel_2T + C_task[0][3]*Xangvel_2T + C_task[0][4]*Yangvel_2T + C_task[0][5]*Zangvel_2T;

	float M1_3T=H_task[0][0]*Xvel_3T + H_task[0][1]*Yvel_3T + H_task[0][2]*Zvel_3T + H_task[0][3]*Xangvel_3T + H_task[0][4]*Yangvel_3T + H_task[0][5]*Zangvel_3T;
	float B1_3T=C_task[0][0]*Xvel_3T + C_task[0][1]*Yvel_3T + C_task[0][2]*Zvel_3T + C_task[0][3]*Xangvel_3T + C_task[0][4]*Yangvel_3T + C_task[0][5]*Zangvel_3T;
*/
	// Without Orientation

	float M1=H_task[0][0]*Xvel + H_task[0][1]*Yvel + H_task[0][2]*Zvel;
	float B1=C_task[0][0]*Xvel + C_task[0][1]*Yvel + C_task[0][2]*Zvel;

	float M1_1T=H_task_1T[0][0]*Xvel_1T + H_task_1T[0][1]*Yvel_1T + H_task_1T[0][2]*Zvel_1T;
	float B1_1T=C_task_1T[0][0]*Xvel_1T + C_task_1T[0][1]*Yvel_1T + C_task_1T[0][2]*Zvel_1T;

	float M1_2T=H_task_2T[0][0]*Xvel_2T + H_task_2T[0][1]*Yvel_2T + H_task_2T[0][2]*Zvel_2T;
	float B1_2T=C_task_2T[0][0]*Xvel_2T + C_task_2T[0][1]*Yvel_2T + C_task_2T[0][2]*Zvel_2T;

	float M1_3T=H_task_3T[0][0]*Xvel_3T + H_task_3T[0][1]*Yvel_3T + H_task_3T[0][2]*Zvel_3T;
	float B1_3T=C_task_3T[0][0]*Xvel_3T + C_task_3T[0][1]*Yvel_3T + C_task_3T[0][2]*Zvel_3T;


	float coeff_9_0 = T*(2*M1*T + B1*pow(T,2) + 12*M1*Tau_x + 6*B1*T*Tau_x);
	float coeff_10_0 = T*(2*M1_1T*T + 3*B1_1T*pow(T,2) - 12*M1_1T*Tau_x + 6*B1_1T*T*Tau_x);
	float coeff_11_0 = T*(-2*M1_2T*T + 3*B1_2T*pow(T,2) - 12*M1_2T*Tau_x - 6*B1_2T*T*Tau_x);
	float coeff_12_0 = T*(-2*M1_3T*T + B1_3T*pow(T,2) + 12*M1_3T*Tau_x - 6*B1_3T*T*Tau_x);

    final_force[0] = (1/coeff_1)*(input_force[0]*coeff_5 
		           + input_force_1T[0]*coeff_6 
				   + input_force_2T[0]*coeff_7 
				   + input_force_3T[0]*coeff_8 
		           - final_force_1T[0]*coeff_2 
				   - final_force_2T[0]*coeff_3 
				   - final_force_3T[0]*coeff_4 
				   - coeff_9_0 
				   - coeff_10_0 
				   - coeff_11_0 
				   - coeff_12_0);
    

    // Y-Direction

	float Tau_y = 1/(2*3.14*10);  // Time-Constant of Q-Filter in Y-Axis

	coeff_1 = 12*T*pow(Tau_y,2) + 8*pow(Tau_y,3);
    coeff_2 = -12*T*pow(Tau_y,2) - 24*pow(Tau_y,3);
	coeff_3 = -12*T*pow(Tau_y,2) + 24*pow(Tau_y,3);
	coeff_4 = 12*T*pow(Tau_y,2) - 8*pow(Tau_y,3);

	coeff_5 = pow(T,3) + 6*pow(T,2)*Tau_y + 12*T*pow(Tau_y,2) + 8*pow(Tau_y,3);
	coeff_6 = 3*pow(T,3) + 6*pow(T,2)*Tau_y - 12*T*pow(Tau_y,2) - 24*pow(Tau_y,3);
	coeff_7 = 3*pow(T,3) - 6*pow(T,2)*Tau_y - 12*T*pow(Tau_y,2) + 24*pow(Tau_y,3);
	coeff_8 = pow(T,3) - 6*pow(T,2)*Tau_y + 12*T*pow(Tau_y,2) - 8*pow(Tau_y,3);
/*	
	// Including Orientation
	
	float M2=H_task[1][0]*Xvel + H_task[1][1]*Yvel + H_task[1][2]*Zvel + H_task[0][3]*Xangvel + H_task[0][4]*Yangvel + H_task[0][5]*Zangvel;
	float B2=C_task[1][0]*Xvel + C_task[1][1]*Yvel + C_task[1][2]*Zvel + C_task[0][3]*Xangvel + C_task[0][4]*Yangvel + C_task[0][5]*Zangvel;

	float M2_1T=H_task[1][0]*Xvel_1T + H_task[1][1]*Yvel_1T + H_task[1][2]*Zvel_1T + H_task[1][3]*Xangvel_1T + H_task[1][4]*Yangvel_1T + H_task[1][5]*Zangvel_1T;
	float B2_1T=C_task[1][0]*Xvel_1T + C_task[1][1]*Yvel_1T + C_task[1][2]*Zvel_1T + C_task[1][3]*Xangvel_1T + C_task[1][4]*Yangvel_1T + C_task[1][5]*Zangvel_1T;

	float M2_2T=H_task[1][0]*Xvel_2T + H_task[1][1]*Yvel_2T + H_task[1][2]*Zvel_2T + H_task[1][3]*Xangvel_2T + H_task[1][4]*Yangvel_2T + H_task[1][5]*Zangvel_2T;
	float B2_2T=C_task[1][0]*Xvel_2T + C_task[1][1]*Yvel_2T + C_task[1][2]*Zvel_2T + C_task[1][3]*Xangvel_2T + C_task[1][4]*Yangvel_2T + C_task[1][5]*Zangvel_2T;

	float M2_3T=H_task[1][0]*Xvel_3T + H_task[1][1]*Yvel_3T + H_task[1][2]*Zvel_3T + H_task[1][3]*Xangvel_3T + H_task[1][4]*Yangvel_3T + H_task[1][5]*Zangvel_3T;
	float B2_3T=C_task[1][0]*Xvel_3T + C_task[1][1]*Yvel_3T + C_task[1][2]*Zvel_3T + C_task[1][3]*Xangvel_3T + C_task[1][4]*Yangvel_3T + C_task[1][5]*Zangvel_3T;
*/
	// Without Orientation

	float M2=H_task[1][0]*Xvel + H_task[1][1]*Yvel + H_task[1][2]*Zvel;
	float B2=C_task[1][0]*Xvel + C_task[1][1]*Yvel + C_task[1][2]*Zvel;

	float M2_1T=H_task_1T[1][0]*Xvel_1T + H_task_1T[1][1]*Yvel_1T + H_task_1T[1][2]*Zvel_1T;
	float B2_1T=C_task_1T[1][0]*Xvel_1T + C_task_1T[1][1]*Yvel_1T + C_task_1T[1][2]*Zvel_1T;

	float M2_2T=H_task_2T[1][0]*Xvel_2T + H_task_2T[1][1]*Yvel_2T + H_task_2T[1][2]*Zvel_2T;
	float B2_2T=C_task_2T[1][0]*Xvel_2T + C_task_2T[1][1]*Yvel_2T + C_task_2T[1][2]*Zvel_2T;

	float M2_3T=H_task_3T[1][0]*Xvel_3T + H_task_3T[1][1]*Yvel_3T + H_task_3T[1][2]*Zvel_3T;
	float B2_3T=C_task_3T[1][0]*Xvel_3T + C_task_3T[1][1]*Yvel_3T + C_task_3T[1][2]*Zvel_3T;
	
	float coeff_9_1 = T*(2*M2*T + B2*pow(T,2) + 12*M2*Tau_y + 6*B2*T*Tau_y);
	float coeff_10_1 = T*(2*M2_1T*T + 3*B2_1T*pow(T,2) - 12*M2_1T*Tau_y + 6*B2_1T*T*Tau_y);
	float coeff_11_1 = T*(-2*M2_2T*T + 3*B2_2T*pow(T,2) - 12*M2_2T*Tau_y - 6*B2_2T*T*Tau_y);
	float coeff_12_1 = T*(-2*M2_3T*T + B2_3T*pow(T,2) + 12*M2_3T*Tau_y - 6*B2_3T*T*Tau_y);

	final_force[1] = (1/coeff_1)*(input_force[1]*coeff_5 
		           + input_force_1T[1]*coeff_6 
				   + input_force_2T[1]*coeff_7 
				   + input_force_3T[1]*coeff_8 
		           - final_force_1T[1]*coeff_2 
				   - final_force_2T[1]*coeff_3 
				   - final_force_3T[1]*coeff_4 
				   - coeff_9_1 
				   - coeff_10_1 
				   - coeff_11_1 
				   - coeff_12_1);

	 // Z-Direction

	float Tau_z = 1/(2*3.14*10);  // Time-Constant of Q-Filter in Z-Axis

	coeff_1 = 12*T*pow(Tau_z,2) + 8*pow(Tau_z,3);
    coeff_2 = -12*T*pow(Tau_z,2) - 24*pow(Tau_z,3);
	coeff_3 = -12*T*pow(Tau_z,2) + 24*pow(Tau_z,3);
	coeff_4 = 12*T*pow(Tau_z,2) - 8*pow(Tau_z,3);

	coeff_5 = pow(T,3) + 6*pow(T,2)*Tau_z + 12*T*pow(Tau_z,2) + 8*pow(Tau_z,3);
	coeff_6 = 3*pow(T,3) + 6*pow(T,2)*Tau_z - 12*T*pow(Tau_z,2) - 24*pow(Tau_z,3);
	coeff_7 = 3*pow(T,3) - 6*pow(T,2)*Tau_z - 12*T*pow(Tau_z,2) + 24*pow(Tau_z,3);
	coeff_8 = pow(T,3) - 6*pow(T,2)*Tau_z + 12*T*pow(Tau_z,2) - 8*pow(Tau_z,3);
/*	
	// Including Orientation
	
	float M3=H_task[2][0]*Xvel + H_task[2][1]*Yvel + H_task[2][2]*Zvel + H_task[0][3]*Xangvel + H_task[0][4]*Yangvel + H_task[0][5]*Zangvel;
	float B3=C_task[2][0]*Xvel + C_task[2][1]*Yvel + C_task[2][2]*Zvel + C_task[0][3]*Xangvel + C_task[0][4]*Yangvel + C_task[0][5]*Zangvel;

	float M3_1T=H_task[2][0]*Xvel_1T + H_task[2][1]*Yvel_1T + H_task[2][2]*Zvel_1T + H_task[2][3]*Xangvel_1T + H_task[2][4]*Yangvel_1T + H_task[2][5]*Zangvel_1T;
	float B3_1T=C_task[2][0]*Xvel_1T + C_task[2][1]*Yvel_1T + C_task[2][2]*Zvel_1T + C_task[2][3]*Xangvel_1T + C_task[2][4]*Yangvel_1T + C_task[2][5]*Zangvel_1T;

	float M3_2T=H_task[2][0]*Xvel_2T + H_task[2][1]*Yvel_2T + H_task[2][2]*Zvel_2T + H_task[2][3]*Xangvel_2T + H_task[2][4]*Yangvel_2T + H_task[2][5]*Zangvel_2T;
	float B3_2T=C_task[2][0]*Xvel_2T + C_task[2][1]*Yvel_2T + C_task[2][2]*Zvel_2T + C_task[2][3]*Xangvel_2T + C_task[2][4]*Yangvel_2T + C_task[2][5]*Zangvel_2T;

	float M3_3T=H_task[2][0]*Xvel_3T + H_task[2][1]*Yvel_3T + H_task[2][2]*Zvel_3T + H_task[2][3]*Xangvel_3T + H_task[2][4]*Yangvel_3T + H_task[2][5]*Zangvel_3T;
	float B3_3T=C_task[2][0]*Xvel_3T + C_task[2][1]*Yvel_3T + C_task[2][2]*Zvel_3T + C_task[2][3]*Xangvel_3T + C_task[2][4]*Yangvel_3T + C_task[2][5]*Zangvel_3T;
*/
	// Without Orientation


	float M3=H_task[2][0]*Xvel + H_task[2][1]*Yvel + H_task[2][2]*Zvel;
	float B3=C_task[2][0]*Xvel + C_task[2][1]*Yvel + C_task[2][2]*Zvel;

	float M3_1T=H_task_1T[2][0]*Xvel_1T + H_task_1T[2][1]*Yvel_1T + H_task_1T[2][2]*Zvel_1T;
	float B3_1T=C_task_1T[2][0]*Xvel_1T + C_task_1T[2][1]*Yvel_1T + C_task_1T[2][2]*Zvel_1T;

	float M3_2T=H_task_2T[2][0]*Xvel_2T + H_task_2T[2][1]*Yvel_2T + H_task_2T[2][2]*Zvel_2T;
	float B3_2T=C_task_2T[2][0]*Xvel_2T + C_task_2T[2][1]*Yvel_2T + C_task_2T[2][2]*Zvel_2T;

	float M3_3T=H_task_3T[2][0]*Xvel_3T + H_task_3T[2][1]*Yvel_3T + H_task_3T[2][2]*Zvel_3T;
	float B3_3T=C_task_3T[2][0]*Xvel_3T + C_task_3T[2][1]*Yvel_3T + C_task_3T[2][2]*Zvel_3T;

	float coeff_9_2 = T*(2*M3*T + B3*pow(T,2) + 12*M3*Tau_z + 6*B3*T*Tau_z);
	float coeff_10_2 = T*(2*M3_1T*T + 3*B3_1T*pow(T,2) - 12*M3_1T*Tau_z + 6*B3_1T*T*Tau_z);
	float coeff_11_2 = T*(-2*M3_2T*T + 3*B3_2T*pow(T,2) - 12*M3_2T*Tau_z - 6*B3_2T*T*Tau_z);
	float coeff_12_2 = T*(-2*M3_3T*T + B3_3T*pow(T,2) + 12*M3_3T*Tau_z - 6*B3_3T*T*Tau_z);

	final_force[2] = (1/coeff_1)*(input_force[2]*coeff_5 
		           + input_force_1T[2]*coeff_6 
				   + input_force_2T[2]*coeff_7 
				   + input_force_3T[2]*coeff_8 
		           - final_force_1T[2]*coeff_2 
				   - final_force_2T[2]*coeff_3 
				   - final_force_3T[2]*coeff_4 
				   - coeff_9_2 
				   - coeff_10_2 
				   - coeff_11_2 
				   - coeff_12_2);

	//fprintf(f,"%f\t\t %f\t\t %f\n", final_force[0], final_force[1], final_force[2]);

	// Swap values with previous time-instants

	for(int i=0; i<=5; i++)
			{
				for(int j=0; j<=5; j++)
				{
					H_task_3T[i][j] = H_task_2T[i][j];
					H_task_2T[i][j] = H_task_1T[i][j];
                    H_task_1T[i][j] = H_task[i][j];

					C_task_3T[i][j] = C_task_2T[i][j];
					C_task_2T[i][j] = C_task_1T[i][j];
                    C_task_1T[i][j] = C_task[i][j];

				}
			}

    

    // Calculate tau = (Cdash*qdot+Fmus(Jtrans*k*posdel))

    tau[0] = Cdash[0]*dq[0] + Fmus[0]*(Jtrans[0][0]*final_force[0] + Jtrans[0][1]*final_force[1] + Jtrans[0][2]*final_force[2]);
    tau[1] = Cdash[1]*dq[1] + Fmus[1]*(Jtrans[1][0]*final_force[0] + Jtrans[1][1]*final_force[1] + Jtrans[1][2]*final_force[2]);
	tau[2] = Cdash[2]*dq[2] + Fmus[2]*(Jtrans[2][0]*final_force[0] + Jtrans[2][1]*final_force[1] + Jtrans[2][2]*final_force[2]);
	tau[3] = Cdash[3]*dq[3] + Fmus[3]*(Jtrans[3][0]*final_force[0] + Jtrans[3][1]*final_force[1] + Jtrans[3][2]*final_force[2]);
    tau[4] = Cdash[4]*dq[4] + Fmus[4]*(Jtrans[4][0]*final_force[0] + Jtrans[4][1]*final_force[1] + Jtrans[4][2]*final_force[2]);
	tau[5] = Cdash[5]*dq[5] + Fmus[5]*(Jtrans[5][0]*final_force[0] + Jtrans[5][1]*final_force[1] + Jtrans[5][2]*final_force[2]);
	tau[6] = Cdash[6]*dq[6] + Fmus[6]*(Jtrans[6][0]*final_force[0] + Jtrans[6][1]*final_force[1] + Jtrans[6][2]*final_force[2]);

	//fprintf(f,"%f\t\t %f\t\t %f\t\t %f\t\t %f\t\t %f\n", tau[0], tau[1], tau[2], tau[3], tau[4], tau[5], tau[6]);
	
	// Add low-pass filter

	float temptau[7] = {0};
	float tempinput_tau[7] = {0};
	float final_tau[7] = {0};
	
	float tc1 = 0.016;
    float tc2 = 0.016;
    float tc3 = 0.016;
    float tc4 = 0.016;
    float tc5 = 0.016;
    float tc6 = 0.016;
    float tc7 = 0.016;

	float tau1=(T*(temptau[0] + tau[0]) + (2*tc1 - T)*tempinput_tau[0])/(2*tc1 + T);
    float tau2=(T*(temptau[1] + tau[1]) + (2*tc2 - T)*tempinput_tau[1])/(2*tc2 + T);
	float tau3=(T*(temptau[2] + tau[2]) + (2*tc3 - T)*tempinput_tau[2])/(2*tc3 + T);
	float tau4=(T*(temptau[3] + tau[3]) + (2*tc4 - T)*tempinput_tau[3])/(2*tc4 + T);
	float tau5=(T*(temptau[4] + tau[4]) + (2*tc5 - T)*tempinput_tau[4])/(2*tc5 + T);
	float tau6=(T*(temptau[5] + tau[5]) + (2*tc6 - T)*tempinput_tau[5])/(2*tc6 + T);
	float tau7=(T*(temptau[6] + tau[6]) + (2*tc7 - T)*tempinput_tau[6])/(2*tc7 + T);

	temptau[0] = tau[0];
	temptau[1] = tau[1];
	temptau[2] = tau[2];
	temptau[3] = tau[3];
	temptau[4] = tau[4];
	temptau[5] = tau[5];
	temptau[6] = tau[6];

	tempinput_tau[0] = tau1;
	tempinput_tau[1] = tau2;
	tempinput_tau[2] = tau3;
	tempinput_tau[3] = tau4;
	tempinput_tau[4] = tau5;
	tempinput_tau[5] = tau6;
	tempinput_tau[6] = tau7;

	final_tau[0] = tau1;
	final_tau[1] = tau2;
	final_tau[2] = tau3;
	final_tau[3] = tau4;
	final_tau[4] = tau5;
	final_tau[5] = tau6;
	final_tau[6] = tau7;

/*
	final_tau[0] = 0;
	final_tau[1] = 0;
	final_tau[2] = 0;
	final_tau[3] = 0;
	final_tau[4] = 0;
	final_tau[5] = 0;
	final_tau[6] = 0;
*/


	//Write final torque values to joint motors

	devices[_T("motor1_left")]->writeDeviceValue(&final_tau[0],1*sizeof(float));
	devices[_T("motor2_left")]->writeDeviceValue(&final_tau[1],1*sizeof(float));
	devices[_T("motor3_left")]->writeDeviceValue(&final_tau[2],1*sizeof(float));
	devices[_T("motor4_left")]->writeDeviceValue(&final_tau[3],1*sizeof(float));
	devices[_T("motor5_left")]->writeDeviceValue(&final_tau[4],1*sizeof(float));
	devices[_T("motor6_left")]->writeDeviceValue(&final_tau[5],1*sizeof(float));
	devices[_T("motor7_left")]->writeDeviceValue(&final_tau[6],1*sizeof(float));

	// Free all variables

	freepointer(C_task,6);
	freepointer(cortemp4,6);
	freepointer(cortemp3,6);
	freepointer(cortemp2,6);
	freepointer(cortemp1,6);

	freepointer(H_task,6);
	freepointer(inertemp2,6);
	freepointer(Jtrans_inv,6);
	freepointer(J_inv,7);
    freepointer(inertemp1,6);
	freepointer(J_Jtrans,6);

	freepointer(C,7);

	freesinglepointer(C7);
	freesinglepointer(C6);
	freesinglepointer(C5);
	freesinglepointer(C4);
	freesinglepointer(C3);
	freesinglepointer(C2);
	freesinglepointer(C1);

	freepointer(H,7);

	freesinglepointer(H7);
	freesinglepointer(H6);
	freesinglepointer(H5);
	freesinglepointer(H4);
	freesinglepointer(H3);
	freesinglepointer(H2);
	freesinglepointer(H1);

	freepointer(Jdot,6);

	freesinglepointer(term14);
	freesinglepointer(term13);
	freesinglepointer(term12);
	freesinglepointer(term11);
	freesinglepointer(term10);
	freesinglepointer(term9);
	freesinglepointer(term8);
	freesinglepointer(term7);
	freesinglepointer(term6);
	freesinglepointer(term5);
	freesinglepointer(term4);
	freesinglepointer(term3);
	freesinglepointer(term2);
	freesinglepointer(term1);

	freesinglepointer(Jdottemp6);
	freesinglepointer(Jdottemp5);
    freesinglepointer(Jdottemp4);
	freesinglepointer(Jdottemp3);
	freesinglepointer(Jdottemp2);
	freesinglepointer(Jdottemp1);

	freesinglepointer(omega_6);
	freesinglepointer(omega_5);
	freesinglepointer(omega_4);
	freesinglepointer(omega_3);
	freesinglepointer(omega_2);
	freesinglepointer(omega_1);
	freesinglepointer(omega_0);

	freesinglepointer(J7dot_upperhalf);
	freesinglepointer(J6dot_upperhalf);
	freesinglepointer(J5dot_upperhalf);
	freesinglepointer(J4dot_upperhalf);
	freesinglepointer(J3dot_upperhalf);
	freesinglepointer(J2dot_upperhalf);
	freesinglepointer(J1dot_upperhalf);

	freesinglepointer(Udot7_6);
	freesinglepointer(Udot6_5);
	freesinglepointer(Udot5_4);
	freesinglepointer(Udot4_3);
	freesinglepointer(Udot3_2);
	freesinglepointer(Udot2_1);
	freesinglepointer(Udot1_0);	

	freesinglepointer(Z6dot);
	freesinglepointer(Z5dot);
	freesinglepointer(Z4dot);
	freesinglepointer(Z3dot);
	freesinglepointer(Z2dot);
	freesinglepointer(Z1dot);
	freesinglepointer(Z0dot);

	freepointer(Jtrans,7);
	freepointer(J,6);

	freesinglepointer(J7_upperhalf);
	freesinglepointer(J6_upperhalf);
	freesinglepointer(J5_upperhalf);
	freesinglepointer(J4_upperhalf);
	freesinglepointer(J3_upperhalf);
	freesinglepointer(J2_upperhalf);
	freesinglepointer(J1_upperhalf);
	freepointer(T7,4);
	freepointer(T6,4);
	freepointer(T5,4);
	freepointer(T4,4);
	freepointer(T3,4);
	freepointer(T2,4);
	freepointer(A7,4);
	freepointer(A6,4);
	freepointer(A5,4);
	freepointer(A4,4);
	freepointer(A3,4);
	freepointer(A2,4);
	freepointer(A1,4);


	rtemp4=NULL;
	rtemp3=NULL;
	rtemp2=NULL;	
	u=NULL;
	rtemp=NULL;
	r=NULL;
	p=NULL;
	v=NULL;
	w=NULL;
	rtemp9=NULL;
	rtemp6=NULL;
	y=NULL;

}


int _tmain(int argc, _TCHAR* argv[])
{
	const rTime delT = 0.001;	// basic clock increment for simulation update

	// make simulation world. 
	PHYSICS_WORLD->createWorld(bContact, delT);
	//PHYSICS_WORLD->setGravity(0, 0, -GRAV_ACC);
	PHYSICS_WORLD->setGravity(0, 0, 0);

	// if simulation includes contact, make ground. 
	if (bContact)
		PHYSICS_WORLD->createPlane(0, 0, 1, 0);

	//create rxNetwork instance to send the data to rPlayer or other data collectors. 
	printf("wait for connection..\n");
	// In order to have rPlayer autoconnected, use the following.
	#ifdef RPLAYER_AUTO_START
	startPlayer("-ip 127.0.0.1 -port 5150");
	#endif
	PHYSICS_WORLD->makeNetwork(1000);
		
	string_type aml =  _T("models\\7_DOF\\kist.aml");	// set your model AML file path (including aml file name)
								// relative to working directory (whose default value is $(RLAB_BIN_PATH))
	string_type name = _T("7_DOF");	// set your model name. Each name should be unique.

	HTransform T0;  // set initial position of your robot. 
	
	dVector q0; // set initial q value of robot. this is optional.
	q0.resize(7);
	q0.zero();

	//q0[0] = 20*DEGREE;
	q0[1] = 90*DEGREE;
	q0[2] = 90*DEGREE;
	q0[3] = -90*DEGREE;
	q0[4] = 90*DEGREE;
    //q0[5] = 20*DEGREE;
	q0[6] = 90*DEGREE;

	rxSystem* sys = NULL;

	sys = PHYSICS_WORLD->createSystem(aml, name, T0, q0);

	map<const string_type, rxDevice*> devices;
	InitializeDevices(sys, devices);

	// make key handler. you can add more key event handler. 
	SetKeyboardHandler();
	
	// Every systems should be created before initialize()...
	PHYSICS_WORLD->initialize();

	// Show Virtual Target Point
/*
	rCustomDrawInfo drawInfo;
	drawInfo.name = "Target";
	drawInfo.flag = CUSTOM_DRAW_NEW;
	drawInfo.drawType = CUSTOM_DRAW_PRIMITIVE_BOX;
	drawInfo.fill = CUSTOM_DRAW_FILLMODE_FILL;
	drawInfo.lineWidth = 1;
	drawInfo.robotId = INVALID_RID;
	drawInfo.bodyId = INVALID_RID;
	drawInfo.T.r[0] = -0.4;
	drawInfo.T.r[1] = 0;
	drawInfo.T.r[2] = 0.5;
	drawInfo.param[0] = 0.03;
	drawInfo.param[1] = 0.03;
	drawInfo.param[2] = 0.03;
	drawInfo.color[0] = 1.0;
	drawInfo.color[1] = 0;
	drawInfo.color[2] = 0;
	drawInfo.color[3] = 1.0;
	rID cid  = PHYSICS_WORLD->customDraw(drawInfo);
*/
	f=fopen("Cartesian_Output.txt","w");
	
	while (!bQuit)
	{
		    UpdateControl(PHYSICS_WORLD->time(), devices);
			t = t + 0.001;
			
			// update simulation. delta time is set to 0.001 now. 
			PHYSICS_WORLD->update();

			// Update Values according to time-instants
			int i;

			for(i=0; i<=2; i++)
			{
				final_force_3T[i] = final_force_2T[i];
				final_force_2T[i] = final_force_1T[i];
				final_force_1T[i] = final_force[i];

				input_force_3T[i] = input_force_2T[i];
				input_force_2T[i] = input_force_1T[i];
				input_force_1T[i] = input_force[i];

			}

			Xvel_3T = Xvel_2T;
			Xvel_2T = Xvel_1T;
			Xvel_1T = Xvel;

			Yvel_3T = Yvel_2T;
			Yvel_2T = Yvel_1T;
			Yvel_1T = Yvel;

			Zvel_3T = Zvel_2T;
			Zvel_2T = Zvel_1T;
			Zvel_1T = Zvel;

			Xangvel_3T = Xangvel_2T;
			Xangvel_2T = Xangvel_1T;
			Xangvel_1T = Xangvel;

            Yangvel_3T = Yangvel_2T;
			Yangvel_2T = Yangvel_1T;
			Yangvel_1T = Yangvel;

            Zangvel_3T = Zangvel_2T;
			Zangvel_2T = Zangvel_1T;
			Zangvel_1T = Zangvel;

			X_3T = X_2T;
			X_2T = X_1T;
			X_1T = X;

			Y_3T = Y_2T;
			Y_2T = Y_1T;
			Y_1T = Y;

			Z_3T = Z_2T;
			Z_2T = Z_1T;
			Z_1T = Z;    

	}

	fclose(f);
	DESTROY_PHYSICS_WORLD();

	// If rPlayer has been autoconnected, 
	// it must be disconnected.
	#ifdef RPLAYER_AUTO_START
	stopPlayer();
	#endif

	return 0;
}

