traj_planner.cpp
// Path planning function, getting the initial and
// the goal configuration of the robot and returning
// a sequence of configurations to reach it, using 
// 3rd order polynomials
Path planning function, getting the initial and
the goal configuration of the robot and returning
a sequence of configurations to reach it, using
3rd order polynomials
Path planning function getting the initial and the goal configuration of the robot and returning a sequence of configurations to reach it using 3rd order polynomials
void traj_plan3()
{
	D_X = GoalX-CMX; 
	D_Y = GoalY-CMY; 
	D_TH = GoalTH-CMpsi; 
			X = CMX+3*D_X*pow((n/(double)timef),2)-2*D_X*pow((n/(double)timef),3);
			Y = CMY+3*D_Y*pow((n/(double)timef),2)-2*D_Y*pow((n/(double)timef),3);
			TH = CMpsi+3*D_TH*pow((n/(double)timef),2)-2*D_TH*pow((n/(double)timef),3);
		for(int i=0; i<(dt+1); i++)
			double j=i;
			X[i] = CMX+CMX_D*j+3*D_X*pow((j/dt),2)-(CMX_DD-CMX_DC)*pow(j,2)/dt-2*D_X*pow((j/dt),3);
			Y[i]=CMY+3*D_Y*pow((j/dt),2)-2*D_Y*pow((j/dt),3);
			TH[i]=CMpsi+3*D_TH*pow((j/dt),2)-2*D_TH*pow((j/dt),3);
	}
	cout<<green<<"Done!!"<<white<<endl;
}
void traj_plan3 ( ) { D_X = GoalX - CMX ; D_Y = GoalY - CMY ; D_TH = GoalTH - CMpsi ; X = CMX + 3 * D_X * pow ( ( n / ( double ) timef ) , 2 ) - 2 * D_X * pow ( ( n / ( double ) timef ) , 3 ) ; Y = CMY + 3 * D_Y * pow ( ( n / ( double ) timef ) , 2 ) - 2 * D_Y * pow ( ( n / ( double ) timef ) , 3 ) ; TH = CMpsi + 3 * D_TH * pow ( ( n / ( double ) timef ) , 2 ) - 2 * D_TH * pow ( ( n / ( double ) timef ) , 3 ) ; cout << green << " " << white << endl ; }
Path planning function, getting the initial and
the goal configuration of the robot and returning
a sequence of configurations to reach it, using
3rd order polynomials
Path planning function getting the initial and the goal configuration of the robot and returning a sequence of configurations to reach it using 3rd order polynomials
// Difference on x-axis // Difference on y-axis // Difference on z-axis (orientation) // Uncomment the if-else section in case you are // interested in particular end-point velocity and // DO NOT insert yes ("Y"/"y") when prompted // if(pauses=='y' || pauses=='Y'); //	{ // Cubic polynomials of time, constructing a sequence // of poses (x,y,theta) //	} //else //{ //	cout<<"Insert the desired end-point velocity vector"<<endl; //	cin>>CMX_DD>>CMY_DD>>CMTH_DD; // //	for(int i=0; i<(dt+1); i++) //	{ //		double j=i; //		X[i] = CMX+CMX_D*j+3*D_X*pow((j/dt),2)-(CMX_DD-CMX_DC)*pow(j,2)/dt-2*D_X*pow((j/dt),3); //		Y[i]=CMY+3*D_Y*pow((j/dt),2)-2*D_Y*pow((j/dt),3); //		TH[i]=CMpsi+3*D_TH*pow((j/dt),2)-2*D_TH*pow((j/dt),3); //	} //}