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 
// 2nd 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
2nd 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 2nd order polynomials
void traj_plan2()
{
	D_X = GoalX-CMX; 
	D_Y = GoalY-CMY; 
	D_TH = GoalTH-CMpsi; 
	X = CMX+D_X*pow((n/(double)timef),2);
	Y = CMY+D_Y*pow((n/(double)timef),2);
	TH = CMpsi+D_TH*pow((n/(double)timef),2);
	cout<<green<<"Done!!"<<white<<endl;
}
void traj_plan2 ( ) { D_X = GoalX - CMX ; D_Y = GoalY - CMY ; D_TH = GoalTH - CMpsi ; X = CMX + D_X * pow ( ( n / ( double ) timef ) , 2 ) ; Y = CMY + D_Y * pow ( ( n / ( double ) timef ) , 2 ) ; TH = CMpsi + D_TH * pow ( ( n / ( double ) timef ) , 2 ) ; 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
2nd 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 2nd order polynomials
// Difference on x-axis // Difference on y-axis // Difference on z-axis (orientation) // Square polynomials of time, constructing a sequence // of poses (x,y,theta)