diff --git a/Core/src/MotionControl/ControlModel.cpp b/Core/src/MotionControl/ControlModel.cpp index 4da6480..17eccdd 100644 --- a/Core/src/MotionControl/ControlModel.cpp +++ b/Core/src/MotionControl/ControlModel.cpp @@ -21,8 +21,6 @@ #include // for Cornell BangBang #include -#include -#include #include #include @@ -80,124 +78,6 @@ void CControlModel::makeCmTrajectory(const PlayerPoseT& start, const PlayerPoseT goto_point_omni(start,final,capability,accel_factor,angle_accel_factor,_nextStep, mode); } -/// Trapezoidal control from ZJU : zero final velocity trajectory -void CControlModel::makeTrapezoidalVelocityPath(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability) -{ - _pathList.clear(); - trapezoidalVelocityPath(start,final,PARAM::Vision::FRAME_RATE,capability,_nextStep,_pathList); -} - -/// Trapezoidal control from ZJU : none-zero final velocity trajectory -void CControlModel::makeNoneTrapezoidalVelocityPath(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability) -{ - _pathList.clear(); - nonetrapezoidalVelocityPath(start,final,PARAM::Vision::FRAME_RATE,capability,_nextStep,_pathList); -} - -/// Parameterized control from ZJU : diff-omni zero final velocity trajectory -//void CControlModel::makePTGTrajectory(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability, PTG_CTRL_MODE mode) -//{ -// // Initialize -// CONTROL_MODULE->Initialize(); -// CONTROL_MODULE->ResetMCState(); -// -// // Two boundary -// P_State initial_state; -// P_State final_state; -// -// static double cmd_v = 0.0; -// static double cmd_w = 0.0; -// -// if (DIFF == mode) { -// // start -// initial_state.x = start.X(); -// initial_state.y = start.Y(); -// initial_state.theta = start.Dir(); // 规划的初始朝向为当前车的朝向 -// initial_state.v = start.Vel().mod(); -// initial_state.w = start.RotVel(); -// // final -// final_state.x = final.X(); -// final_state.y = final.Y(); -// final_state.theta = final.Dir(); -// final_state.v = final.Vel().mod(); -// final_state.w = final.RotVel(); -// -// CONTROL_MODULE->SetTransitionSpeed(initial_state.v); -// CONTROL_MODULE->SetRotationalSpeed(initial_state.w); -// CONTROL_MODULE->GenerateMoveCmd(initial_state,final_state); -// -// // get result -// bool isFailed = CONTROL_MODULE->IS_Generate_PTG_Failed(); -// P_PathList _ptg_pathlist = CONTROL_MODULE->GetPathList(); -// -// _pathList.clear(); -// for (size_t i = 0 ; i < _ptg_pathlist.size(); i ++) { -// vector storeData; -// storeData.push_back(_ptg_pathlist[i].x); -// storeData.push_back(_ptg_pathlist[i].y); -// storeData.push_back(_ptg_pathlist[i].theta); -// storeData.push_back(_ptg_pathlist[i].v); -// storeData.push_back(_ptg_pathlist[i].w); -// storeData.push_back(_ptg_pathlist[i].a); -// _pathList.push_back(storeData); -// } -// -// cmd_v = CONTROL_MODULE->GetTransitionSpeed(); -// cmd_w = CONTROL_MODULE->GetRotationalSpeed(); -// -// _nextStep.SetVel(Utils::Polar2Vector(cmd_v,start.Dir())); -// _nextStep.SetRotVel(cmd_w); -// -// } else if (OMNI == mode) { -// initial_state.x = start.X(); -// initial_state.y = start.Y(); -// initial_state.theta = start.Vel().dir(); // 规划的初始朝向为当前车速度的方向 -// initial_state.v = start.Vel().mod(); -// initial_state.w = start.RotVel(); -// -// if (fabs(initial_state.v) < 10) { -// initial_state.theta = (final.Pos() - start.Pos()).dir(); -// initial_state.v = 10; -// } -// -// final_state.x = final.X(); -// final_state.y = final.Y(); -// final_state.theta = final.Dir(); -// final_state.v = final.Vel().mod(); -// final_state.w = final.RotVel(); -// -// CONTROL_MODULE->SetTransitionSpeed(initial_state.v); -// CONTROL_MODULE->SetRotationalSpeed(initial_state.w); -// CONTROL_MODULE->GenerateMoveCmd(initial_state,final_state); -// -// // get result -// bool isFailed = CONTROL_MODULE->IS_Generate_PTG_Failed(); -// P_PathList _ptg_pathlist = CONTROL_MODULE->GetPathList(); -// -// _pathList.clear(); -// for (size_t i = 0 ; i < _ptg_pathlist.size(); i ++) { -// vector storeData; -// storeData.push_back(_ptg_pathlist[i].x); -// storeData.push_back(_ptg_pathlist[i].y); -// storeData.push_back(_ptg_pathlist[i].theta); -// storeData.push_back(_ptg_pathlist[i].v); -// storeData.push_back(_ptg_pathlist[i].w); -// storeData.push_back(_ptg_pathlist[i].a); -// _pathList.push_back(storeData); -// } -// -// -// cmd_v = CONTROL_MODULE->GetTransitionSpeed(); -// cmd_w = CONTROL_MODULE->GetRotationalSpeed(); -// -// _nextStep.SetVel(Utils::Polar2Vector(cmd_v,Utils::Normalize(initial_state.theta+cmd_w*1.0/60))); -// _nextStep.SetRotVel(1.0*cmd_w); -// -// } -// -// return ; -//} - ////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////// diff --git a/Core/src/MotionControl/ControlModel.h b/Core/src/MotionControl/ControlModel.h index 7ba3d40..4133e06 100644 --- a/Core/src/MotionControl/ControlModel.h +++ b/Core/src/MotionControl/ControlModel.h @@ -53,15 +53,6 @@ class CControlModel { /// Trapezoidal control from CMU : none-zero final velocity trajectory void makeCmTrajectory(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability, nonZeroMode mode = FAST); - /// Trapezoidal control from ZJU : zero final velocity trajectory - void makeTrapezoidalVelocityPath(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability); - - /// Trapezoidal control from ZJU : none-zero final velocity trajectory - void makeNoneTrapezoidalVelocityPath(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability); - - /// Parameterized control from ZJU : diff-omni zero final velocity trajectory - // void makePTGTrajectory(const PlayerPoseT& start, const PlayerPoseT& final, const PlayerCapabilityT& capability, PTG_CTRL_MODE mode = DIFF); - /// Get the real-time next step const PlayerPoseT& getNextStep() const { return _nextStep; } diff --git a/Core/src/MotionControl/TrapezoidalVelTrajectory.cpp b/Core/src/MotionControl/TrapezoidalVelTrajectory.cpp deleted file mode 100644 index 952ec73..0000000 --- a/Core/src/MotionControl/TrapezoidalVelTrajectory.cpp +++ /dev/null @@ -1,983 +0,0 @@ -#include -#include -#include -#include "staticparams.h" -#include -#include -#include -#include -#include "staticparams.h" -#include "TrapezoidalVelTrajectory.h" - - -//#define _PRINT_DEBUG_MSG -#ifdef _PRINT_DEBUG_MSG -#define PRINT_DEBUG(x) std::cout << #x << " : " << (x) << std::endl; -#define PRINT_MSG(x) std::cout << x << std::endl; -#else -#define PRINT_DEBUG(x) -#define PRINT_MSG(x) -#endif - -//static CVector VEL_LOG[2] = {CVector(-9999,-9999), CVector(-9999, -9999)}; -//////////////////////////////////////////////////////////////////////////////////////////////////// -/// @fn vector> tg_recurs(double t0, double z0, double zf, double vz0, int rev, -/// int iter, double uza, double uzd, double vzMax) -/// -/// @brief 零速到点规划。基本算法 -/// -/// 根据 t0 时刻的机器人状态，计算得到从起点到终点之间的整个规划过程。 -/// 该方法假设机器人在加速和减速过程中，机器人的加速度是恒定不变的。 -/// 在这个假设的基础上，根据机器人的初始状态, 将机器人的运动分为三 -/// 大类： -/// 1: 初速度与位移方向不一致，先将减速到 0 -/// 2: 初速度与位移方向一致 -/// 2.1: 初速度小于最大速度 Vmax, 且加速至最大速度时位移不会 -/// 超过目标点，则先加速至最大速度。 -/// 2.2: 保持最大速度运动 -/// 2.3: 以最大减速度减速，也会超过目标点，则以最大减速减速 -/// 3: 初速大于最大速度，先减速到 Vmax -/// -/// -/// -/// @author Yonghai Wu -/// @date 2009-12-9 -/// -/// @param t0 initial time = absolute time when this stage begins -/// @param z0 initial position -/// @param zf final position -/// @param vz0 initial velocity. -/// @param rev The reverse. -/// @param iter The iterator. -/// @param uza the acceleration. -/// @param uzd The decelerate. -/// @param vzMax The vz maximum. -/// -/// @return 返回规划得到的路径，路径由多段组成，每段包括以下参数： -/// -/// z0U: 初始位移 -/// vz0U: 下一阶段的初速度 -/// az: 这一阶段的加速度 -/// rev: 是否改变参考方向 -/// t: 这一阶段所用时间 -/// t0U: 已用时间（不包括这一阶段） -/// -//////////////////////////////////////////////////////////////////////////////////////////////////// - -vector> tg_recurs(double t0, - double z0, - double zf, - double vz0, - int rev, - int iter, - double uza, - double uzd, - double vzMax) -{ - //std::cout<<"TrapezoidalVelTrajectory"< > DataVector; - iter = iter + 1; - if (abs(z0)!=0) - { - // Adjust offset for z0 - zfU = zf-z0; - z0U = 0; - vz0U = vz0; - az = 0; - t = 0; - } - while ( (abs(zfU)>=tol) || (abs(vz0U)>=tol) ) - { - //std::cout<<"zfU:"< vzMax ){ - //Case 3: Need to decelerate until vz = vzMax - PRINT_MSG("Case 3"); - t = (vz0U-vzMax)/uzd; - z0U = 0; - zfU = zfU - (vz0U*vz0U-vzMax*vzMax)/2.0/uzd; - vz0U = vzMax; - az = -uzd; - } - else if (vz0U<0){ - // 需要反向，先把原来的速度减到零 - //Case 1: Moving in wrong direction, need to decelerate first - PRINT_MSG("Case 1"); - t = abs(vz0U/uzd); - double zDelta = vz0U*t + uzd*t*t/2.0; - z0U = 0; - zfU = zfU-zDelta; - vz0U = 0; // vz0 + uzd*t; // "correct" solution isn't great, rounding errors - az = uzd; - } - else if (zfU<=(vz0U*vz0U/2.0/uzd + tol)){ - // 从现在起以最大减速度减速，也会超过目标点 - // Case 2.3: Need to decelerate - PRINT_MSG("Case 2.3"); - t = vz0U/uzd; - double zDelta = vz0U*vz0U/2.0/uzd; - z0U = 0; - zfU = zfU-zDelta; - vz0U = 0; - az = -uzd; - } - else if ( fabs(vz0U - vzMax) < tol ){ - // 计算保持最大速度的时间，这段时间运动的距离，及到减速时剩下的路程 - //Case 2.2: Cruising at vzMax until you have to decelerate - PRINT_MSG("Case 2.2"); - z0U = 0; - double zDelta = vzMax*vzMax/2.0/uzd; - vz0U = vzMax; - az = 0; - double zCruise = zfU - zDelta; - t = zCruise / vzMax; - zfU = vzMax*vzMax/2.0/uzd; - PRINT_DEBUG(t); - } - else{ - // Case 2.1: Accelerate - PRINT_MSG("Case 2.1"); - // Accelerate to vzMax - double t1 = (vzMax - vz0U)/uza; - - // Accelerate without reaching vzMax - double zDelta = vz0U*vz0U/2.0/uza; - float temp = (float)( (zfU + zDelta)/(1.0/2/uza + 1.0/2/uzd) ); - double vz2 = Utils::SquareRootFloat( temp ); - double t2 = (vz2-vz0U)/uza; - if (t1 storeData(data,data+6); - DataVector.push_back(storeData); - t0U = t + t0U; //Initial time for the next stage - - if ( vz0U == vzMax && vzMax < 0 ) - break; - } - double data[6] = {0, 0, 0, rev, 0, t0U}; - vector storeData(data,data+6); - DataVector.push_back(storeData); - return DataVector; -} - -//////////////////////////////////////////////////////////////////////////////////////////////////// -/// @fn vector< vector > tg_recurs(double t0, double z0, double zf, double vz0, -/// double vzf, int rev, int iter, double uza, double uzd, double vzMax) -/// -/// @brief 非零速到点规划。 -/// -/// ???? 对目标的设定超过机器人的运动学约束的情况，考虑还不是很细致，有可能会出问题。 -/// 根据 t0 时刻的机器人状态，计算得到从起点到终点之间的整个规划过程。 -/// 该方法假设机器人在加速和减速过程中，机器人的加速度是恒定不变的。 -/// 在这个假设的基础上，根据机器人的初始状态, 将机器人的运动分为三 -/// 大类： -/// 1: 初速度与位移方向不一致，先将减速到 0 -/// 2: 初速度与位移方向一致 -/// 2.1: 初速度小于最大速度 Vmax, 且加速至最大速度时位移不会 -/// 超过目标点，则先加速至最大速度。 -/// 2.2: 保持最大速度运动 -/// 2.3: 以最大减速度减速，也会超过目标点，则以最大减速减速 -/// 3: 初速大于最大速度，先减速到 Vmax -/// -/// @author Yonghai Wu -/// @date 2009-12-9 -/// -/// @param t0 initial time = absolute time when this stage begins -/// @param z0 initial position -/// @param zf final position -/// @param vz0 initial velocity. -/// @param vzf final velocity. -/// @param rev The reverse. -/// @param iter The iterator. -/// @param uza the acceleration. -/// @param uzd The decelerate. -/// @param vzMax The vz maximum. -/// -/// @return 返回规划得到的路径，路径由多段组成，每段包括以下参数： -/// -/// z0U: 初始位移 -/// vz0U: 下一阶段的初速度 -/// az: 这一阶段的加速度 -/// rev: 是否改变参考方向 -/// t: 这一阶段所用时间 -/// t0U: 已用时间（不包括这一阶段）. -//////////////////////////////////////////////////////////////////////////////////////////////////// - -vector< vector > tg_recurs(double t0, - double z0, - double zf, - double vz0, - double vzf, - int rev, - int iter, - double uza, - double uzd, - double vzMax) -{ - // zfU: 剩余路程 - // vz0: 初速度 - // vz0U: 下一阶段的初速度 - // az: 这一阶段的加速度 - // rev: 是否改变参考方向 - // t: 这一阶段所用时间 - // t0U: 已用时间（不包括这一阶段） - const double tol = 0.01; // tolerance for machine precision - const double pos_tol = vzMax/1000; - const double vel_tol = vzMax/1000; - double zfU = zf; - double z0U = z0; - double vz0U = vz0; - double az; - double t=0; - double t0U = 0; - vector > DataVector; - PRINT_DEBUG(z0); - PRINT_DEBUG(zf); - PRINT_DEBUG(vz0); - PRINT_DEBUG(vzf); - iter = iter + 1; - int numOfStep = 0; // -// bool canReach = true; - - if (fabs(z0)!=0) - { - // Adjust offset for z0 - zfU = zf-z0; - z0U = 0; - vz0U = vz0; - az = 0; - t = 0; - } - /// check if the robot can reach the target - - while ( ((fabs(zfU)>= pos_tol) || (fabs(vz0U-vzf)>= vel_tol)) && (numOfStep < 10) ) - { - //std::cout<<"zfU:"< storeData(data,data+6); - DataVector.push_back(storeData); - t0U = t + t0U; //Initial time for the next stage - - if ( vz0U == vzMax && vzMax < 0 ) - break; - } - double data[6] = {0, 0, 0, rev, 0, t0U}; - vector storeData(data,data+6); - DataVector.push_back(storeData); - return DataVector; -} - -//////////////////////////////////////////////////////////////////////////////////////////////////// -/// @fn void buildPath(const vector>& storeData,double& z, double& vz, -/// double& azList) -/// -/// @brief 根据函数 的结果返回下一周期的位移 z，速度 vz 和加速度 azList。 -/// -/// 1. 根据算法的基本假设，加速度恒定，因此加速度 a = storeData[stage-1][2]; -/// 2. 速度 v = v0 + a*dt -/// 3. 位移 z= z0 + v*dt -/// -/// @author Yonghai Wu -/// @date 2009-12-9 -/// -/// @param storeData Information describing the pathe. -/// @param [in,out] z the next postion. -/// @param [in,out] vz the next velocity. -/// @param [in,out] azList the next acceration. -//////////////////////////////////////////////////////////////////////////////////////////////////// - -void buildPath(const vector>& storeData,double& z, double& vz, double& azList) -{ - double num_stages = storeData.size(); - vector data = storeData[num_stages-1]; - double Duration = data[5]; // 运动控制总时间 - int stage = 0; - z = storeData[0][0]; - int tfint = Duration*PARAM::Vision::FRAME_RATE; - int maxStepNumber=(std::min)(tfint, 1); - for (int index = 0;index<=maxStepNumber;index++){ - if (index>=Duration*PARAM::Vision::FRAME_RATE) - continue; - while (index>=storeData[stage][5]*PARAM::Vision::FRAME_RATE) - stage = stage + 1; - double t0 = storeData[stage-1][5] ; - double vz0 = storeData[stage-1][1]; - double az = storeData[stage-1][2]; - double rev = storeData[stage-1][3]; - azList = rev*az; - vz = rev*vz0 + rev*az * (index*1.0/PARAM::Vision::FRAME_RATE-t0); - if (index!=0) - z+=vz0/PARAM::Vision::FRAME_RATE; - } - // This is to prevent errors when duration = 0 - if (tfint == 0){ - //std::cout<<"duration = 0!!!!!"< >& storeData, double& z, double& vz, -/// double& azList, vector< vector >& pathList) -/// -/// 根据函数 的结果返回下一周期的位移 z，速度 vz 和加速度 azList 和整条路径 pathList。 -/// -/// 1. 根据算法的基本假设，加速度恒定，因此加速度 a = storeData[stage-1][2]; -/// 2. 速度 v(t) = v0 + a*dt -/// 3. 位移 z(t) = z0 + v(t)*dt -/// -/// @author Yonghai Wu -/// @date 2009-12-9 -/// -/// @param storeData Information describing the store. -/// @param [in,out] z the z coordinate. -/// @param [in,out] vz the vz. -/// @param [in,out] azList list of azs. -/// @param [in,out] pathList list of pathes. -//////////////////////////////////////////////////////////////////////////////////////////////////// - -void buildPath(const vector< vector >& storeData, - double& z, - double& vz, - double& azList, - vector< vector >& pathList) -{ - double num_stages = storeData.size(); - vector data = storeData[num_stages-1]; - double Duration = data[5]; // 运动控制总时间 - int stage = 0; - double z_t, vz_t, azList_t; - z_t = storeData[0][0]; - int tfint = Duration*PARAM::Vision::FRAME_RATE; - int maxStepNumber = (std::min)( int(PARAM::Vision::FRAME_RATE), (std::max)(tfint, 1)); - int indexStep = 6; - for (int index = -1;index<=maxStepNumber;){ - if( index < 1 ) - index ++; - else - index += indexStep; - - if (index>=Duration*PARAM::Vision::FRAME_RATE) - continue; - while (index>=storeData[stage][5]*PARAM::Vision::FRAME_RATE) - stage = stage + 1; - double t0 = storeData[stage-1][5]; - double vz0 = storeData[stage-1][1]; - double az = storeData[stage-1][2]; - double rev = storeData[stage-1][3]; - azList_t = rev*az; - vz_t = rev*vz0 + rev*az * (index*1.0/PARAM::Vision::FRAME_RATE-t0); - if (index!=0 ){ - if( index == 1 ) - z_t+=vz_t/PARAM::Vision::FRAME_RATE; - else - z_t+=indexStep*vz_t/PARAM::Vision::FRAME_RATE; - } - - if ( 1 == index) - { - z = z_t; - vz = vz_t; - azList = azList_t; - //std::cout<<"stage:"< storedata(data, data+3); - pathList.push_back(storedata); - // std::cout<<"stage:"< > storeDataX = tg_recurs(t0, x0, xf, vx0, rev, iter, axMax, factor*axMax, vxMax); - double num_stagesX = storeDataX.size(); - durX = storeDataX[num_stagesX-1][5]; - //Find duration for y-direction - iter = 0; - t0 = 0; - rev = 1; - vector > storeDataY = tg_recurs(t0, y0, yf, vy0, rev, iter, ayMax, factor*ayMax, vyMax); - double num_stagesY = storeDataY.size(); - durY = storeDataY[num_stagesY-1][5]; - double tfDelta = durX-durY; - //std::cout<<"alpha:"< PARAM::Math::PI) - dtheta = dtheta - 2*PARAM::Math::PI; - thetaf=theta0 + dtheta; - - //double controlAlpha = syncTG(x0,y0,vx0,vy0,maxAccel,maxAccel,maxSpeed); - // Do a binary search to find the proper control effort, such that the times for the - // trajectories in x and y direction are the same. - // The variable to vary is "alpha", which runs from 0 to pi/2 - - const double factor = 1.5; - - int numIter = 5; //number of iterations performed. error in u is 2^(-numIter) - double u = 1.568; // approx. PI/2 - double du = -1.568; - double alpha,durX,durY; - vector > storeDataX,storeDataY; - int iter = 0; - double t0 = 0; - double rev = 1; - for (int j=1;j<=numIter;j++) - { - du = du*0.5; - alpha = u+du; - double axMax = sin(alpha)*maxAccel; - double ayMax = cos(alpha)*maxAccel; - double vxMax = sin(alpha)*maxSpeed; - double vyMax = cos(alpha)*maxSpeed; - //std::cout< > storeDataTheta = tg_recurs(t0,theta0,thetaf,vtheta0,rev,iter,maxAngleAccel,factor*maxAngleAccel,maxAngleSpeed); - buildPath(storeDataTheta,theta,vtheta,atheta); - buildPath(storeDataX,x,vx,ax); - buildPath(storeDataY,y,vy,ay); -// double duration = durX > durY ? durX : durY; // 运动控制总时间 - nextStep.SetValid(true); - nextStep.SetPos(x,y); - nextStep.SetVel(vx,vy); - nextStep.SetDir(theta); - nextStep.SetRotVel(vtheta); - -} - -//////////////////////////////////////////////////////////////////////////////////////////////////// -/// @fn void trapezoidalVelocityPath( const PlayerPoseT& start, const PlayerPoseT& final, -/// double frameRate, const PlayerCapabilityT& capability, PlayerPoseT& nextStep, -/// vector< vector >& pathList) -/// -/// @brief 非零速到点路径规划函数。算法流程如下： -/// 1. 采用二分法，同步 x 和 y 方向的位移 -/// 2. 独立计算转动方向的所化时间 t_theta, 取 t_x, t_y, t_theta 中的最大值 -/// 为最后所花的时间 -/// -/// @author Yonghai Wu -/// @date 2010-3-24 -/// -/// @param start The start. -/// @param final The final. -/// @param frameRate The frame rate. -/// @param capability The capability. -/// @param [in,out] nextStep the next step. 下一周期的位移和速度 -/// @param [in,out] pathList list of pathes. 整条路径 -//////////////////////////////////////////////////////////////////////////////////////////////////// - -void trapezoidalVelocityPath( const PlayerPoseT& start, - const PlayerPoseT& final, - double frameRate, - const PlayerCapabilityT& capability, - PlayerPoseT& nextStep, - vector< vector >& pathList) - { - - - double x0 = start.X(); - double y0 = start.Y(); - double xf = final.X(); - double yf = final.Y(); - double vx0 = start.VelX(); - double vy0 = start.VelY(); - double vxf = final.VelX(); - double vyf = final.VelY(); - double vtheta0 = start.RotVel(); - double vthetaf = final.RotVel(); - double theta0 = start.Dir(); - double thetaf = final.Dir(); - - double x,vx,ax,y,vy,ay,theta,vtheta,atheta; - if (capability.maxAccel==0 - || capability.maxAngularAccel==0 - || capability.maxSpeed==0 - || capability.maxAngularSpeed==0){ - cout << "trapezoidalVelocityPath code error: invalid scale(s)" << endl; - } - /*std::cout<<" maxacc: "< PARAM::Math::PI) - dtheta = dtheta - 2*PARAM::Math::PI; - thetaf=theta0 + dtheta; - - //double controlAlpha = - // syncTG(x0,y0,vx0,vy0,maxAccel,maxAccel,maxSpeed); Do a binary - // search to find the proper control effort, such that the times - // for the trajectories in x and y direction are the same. The - // variable to vary is "alpha", which runs from 0 to pi/2 - - // syncTG(start, final, capability); - - - int numIter = 5; //number of iterations performed. error in u is 2^(-numIter) - double u = 1.568; // approx. PI/2 - double du = -1.568; - double alpha,durX,durY; - vector > storeDataX,storeDataY; - int iter = 0; - double t0 = 0; - double rev = 1; - - double tol_pos = 1; - double tol_vel = 2; - double tol_theta = 0.01; - double tol_vtheta = 0.02; - for (int j=1;j<=numIter;j++) - { - - - du = du*0.5; - alpha = u+du; - //alpha = PI/2; - double axMax = sin(alpha)*maxAccel; - double ayMax = cos(alpha)*maxAccel; - double adxMax = sin(alpha)*maxDec; - double adyMax = cos(alpha)*maxDec; - double vxMax = sin(alpha)*maxSpeed; - double vyMax = cos(alpha)*maxSpeed; - //std::cout< > storeDataTheta = tg_recurs(t0, - theta0, - thetaf, - vtheta0, - vthetaf, - rev, - iter, - maxAngleAccel, - maxAngleDec, - maxAngleSpeed, - tol_theta, - tol_vtheta); - - - - //cout<<"theta: "< > pathListR, pathListX, pathListY; - buildPath(storeDataTheta,theta,vtheta,atheta,pathListR); - buildPath(storeDataX,x,vx,ax, pathListX); - buildPath(storeDataY,y,vy,ay, pathListY); - -// double duration = durX > durY ? durX : durY; // 运动控制总时间 - - int pathStepX = pathListX.size(); - int pathStepY = pathListY.size(); - int pathStepR = pathListR.size(); - //std::cout<<" path len: r "<= pathStepX) - { - data[1] = 0; - data[4] = 0; - } - else{ - data[1] = pathListX[i][0]; - data[4] = pathListX[i][1]; - } - if ( i >= pathStepY) - { - data[2] = 0; - data[5] = 0; - } - else{ - data[2] = pathListY[i][0]; - data[5] = pathListY[i][1]; - } - - if ( i >= pathStepR) - { - data[3] = 0; - data[6] = 0; - } - else{ - data[3] = pathListR[i][0]; - data[6] = pathListR[i][1]; - } - vector storeData(data, data+7); - pathList.push_back(storeData); - // PRINT_DEBUG(pathList.size()); - } - - - - } diff --git a/Core/src/MotionControl/TrapezoidalVelTrajectory.h b/Core/src/MotionControl/TrapezoidalVelTrajectory.h deleted file mode 100644 index d0b124e..0000000 --- a/Core/src/MotionControl/TrapezoidalVelTrajectory.h +++ /dev/null @@ -1,61 +0,0 @@ -#ifndef TRAPEZOIDAL_VEL_TRAJECTORY_H -#define TRAPEZOIDAL_VEL_TRAJECTORY_H -#include -#include - - - -using namespace std; -vector> tg_recurs(double t0, - double z0, - double zf, - double vz0, - int rev, - int iter, - double uza, - double uzd, - double vzMax); - -vector> tg_recurs(double t0, - double z0, - double zf, - double vz0, - double vzf, - int rev, - int iter, - double uza, - double uzd, - double vzMax, - double tol_pos, - double tol_vel); - -void buildPath(const vector< vector > & storeData, - double& z, - double& vz, - double& azList, - vector< vector >& pathList); - -void buildPath(const vector> & storeData, - double& z, - double& vz, - double& azList); - -double syncTG(const PlayerPoseT& start, - CGeoPoint finalPos, - const PlayerCapabilityT& capability); - -void trapezoidalVelocityPath( const PlayerPoseT& start, - const PlayerPoseT& final, - double frameRate, - const PlayerCapabilityT& capability, - PlayerPoseT& nextStep - ); - -void trapezoidalVelocityPath( const PlayerPoseT& start, - const PlayerPoseT& final, - double frameRate, - const PlayerCapabilityT& capability, - PlayerPoseT& nextStep, - vector< vector >& pathList - ); -#endif \ No newline at end of file diff --git a/Core/src/MotionControl/noneTrapzodalVelTrajectory.cpp b/Core/src/MotionControl/noneTrapzodalVelTrajectory.cpp deleted file mode 100644 index 3a37a8a..0000000 --- a/Core/src/MotionControl/noneTrapzodalVelTrajectory.cpp +++ /dev/null @@ -1,1016 +0,0 @@ -#include -#include -#include -#include "staticparams.h" -#include -#include -#include -#include -#include "noneTrapzodalVelTrajectory.h" - - -//#define _PRINT_DEBUG_MSG -#ifdef _PRINT_DEBUG_MSG -#define PRINT_DEBUG(x) std::cout << #x << " : " << (x) << std::endl; -#define PRINT_MSG(x) std::cout << x << std::endl; -#else -#define PRINT_DEBUG(x) -#define PRINT_MSG(x) -#endif -namespace{ - bool SHOW_TG_TIME = true; - -} -//static CVector VEL_LOG[2] = {CVector(-9999,-9999), CVector(-9999, -9999)}; -///////////////////////////////////////////////////////////////////// -/// @fn vector> genTrajZero -/// -/// @brief 零速到点规划。基本算法 -/// -/// -/// -/// @author Yonghai Wu -/// @date 2009-12-9 -/// -/// @param t0 initial time = absolute time when this stage begins -/// @param z0 initial position -/// @param zf final position -/// @param vz0 initial velocity. -/// @param rev The reverse. -/// @param iter The iterator. -/// @param uza the acceleration. -/// @param uzd The decelerate. -/// @param vzMax The vz maximum. -/// -/// @return 返回规划得到的路径，路径由多段组成，每段包括以下参数： -/// -/// z0U: 初始位移 -/// vz0U: 下一阶段的初速度 -/// az: 这一阶段的加速度 -/// rev: 是否改变参考方向 -/// t: 这一阶段所用时间 -/// t0U: 已用时间（不包括这一阶段） -/// -/////////////////////////////////////////////////////////////////////////////////////// - -vector> genTrajZero(vector> &DataVector, - double zfU, - double vz0, - int rev, - double uza, - double uzd, - double vzMax, - double tol_pos, - double tol_vel) -{ - //std::cout<<"TrapezoidalVelTrajectory"< 0 ) - { - t0U = DataVector[numOfDataVector-1][5]; - } - double zDelta = 0; - - while ( (abs (zfU ) >= pos_factor*tol_pos) || ( abs( vz0U ) >= vel_factor*tol_vel ) ) - { - //std::cout<<"zfU:"< vzMax ){ - //Case 3: Need to decelerate until vz = vzMax - PRINT_MSG("Case 3"); - t = ( vz0U - vzMax ) / uzd; - z0U = 0; - zDelta = ( vz0U*vz0U - vzMax*vzMax ) / 2.0 / uzd; - zfU = zfU - zDelta; - vz0U = vzMax; - az = -uzd; - } - else if ( vz0U < 0 ){ - // 需要反向，先把原来的速度减到零 - //Case 1: Moving in wrong direction, need to decelerate first - PRINT_MSG("Case 1"); - t = abs( vz0U / uzd ); - zDelta = vz0U*t + uzd*t*t/2.0; - z0U = 0; - zfU = zfU-zDelta; - vz0U = 0; // vz0 + uzd*t; // "correct" solution isn't great, rounding errors - az = uzd; - } - else if (zfU <= ( vz0U*vz0U / 2.0/ uzd + tol_pos )){ - // 从现在起以最大减速度减速，也会超过目标点 - // Case 2.3: Need to decelerate - PRINT_MSG("Case 2.3"); - t = vz0U / uzd; - zDelta = vz0U*vz0U/2.0/uzd; - z0U = 0; - zfU = zfU-zDelta; - vz0U = 0; - az = -uzd; - } - else if ( fabs(vz0U - vzMax) < tol_vel ){ - // 计算保持最大速度的时间，这段时间运动的距离，及到减速时剩下的路程 - //Case 2.2: Cruising at vzMax until you have to decelerate - PRINT_MSG("Case 2.2"); - z0U = 0; - zDelta = vzMax*vzMax/2.0/uzd; - vz0U = vzMax; - az = 0; - double zCruise = zfU - zDelta; - t = zCruise / vzMax; - zfU = vzMax*vzMax/2.0/uzd; - } - else{ - // Case 2.1: Accelerate - PRINT_MSG("Case 2.1"); - // Accelerate to vzMax - double t1 = (vzMax - vz0U)/uza; - - // Accelerate without reaching vzMax - zDelta = vz0U*vz0U/2.0/uza; - float temp = (float)( (zfU + zDelta)/(1.0/2/uza + 1.0/2/uzd) ); - double vz2 = sqrt( temp ); - double t2 = (vz2-vz0U)/uza; - if (t1 storeData(data,data+6); - DataVector.push_back(storeData); - t0U = t + t0U; //Initial time for the next stage - - if ( vz0U == vzMax && vzMax < 0 ) - break; - } - - double data[6] = {0, 0, 0, rev, 0, t0U}; - vector storeData(data,data+6); - DataVector.push_back(storeData); - return DataVector; -} - -/////////////////////////////////////////////////////////////////////////////////////// -/// @fn -/// -/// @brief 非零速到点规划。 -/// -/// @author Yonghai Wu -/// @date 2009-12-9 -/// -/// @param t0 initial time = absolute time when this stage begins -/// @param z0 initial position -/// @param zf final position -/// @param vz0 initial velocity. -/// @param vzf final velocity. -/// @param rev The reverse. -/// @param iter The iterator. -/// @param uza the acceleration. -/// @param uzd The decelerate. -/// @param vzMax The vz maximum. -/// -/// @return 返回规划得到的路径，路径由多段组成，每段包括以下参数： -/// -//////////////////////////////////////////////////////////////////////////////// -vector< vector > genTrajNone(vector> &DataVector, - double zfU, - double vz0, - double vzf, - int rev, - double uza, - double uzd, - double vzMax, - double tol_pos, - double tol_vel) -{ - // zfU: 剩余路程 - // vz0: 初速度 - // vz0U: 下一阶段的初速度 - // az: 这一阶段的加速度 - // rev: 是否改变参考方向 - // t: 这一阶段所用时间 - // t0U: 已用时间（不包括这一阶段） - const double tol = 0.01; // tolerance for machine precision - double z0U = 0; - double vz0U = vz0; - double az; - double t=0; - double t0U = 0; - int numOfDataVector = DataVector.size() ; - if ( numOfDataVector > 0 ) - { - t0U = DataVector[numOfDataVector-1][5]; - } - - int numOfStep = 0; // -// bool canReach = true; - double zDelta = 0; - - /// check if the robot can reach the target - - while ( ((fabs(zfU)>= 1.2*tol_pos) || (fabs(vz0U-vzf)>= 1.2*tol_pos)) && (numOfStep < 5) ) - { - vz0 = vz0U; - if ( numOfStep > 4 ) - std::cout<<"~~~~~~~~~~~~~~~~~~~~~~~~~~zfU:"< vzMax ){ - // 需要反向，先把原来的速度减到零 - PRINT_MSG("Case 3"); - t = (vz0U-vzMax)/uzd; - z0U = 0; - zDelta = (vz0U*vz0U-vzMax*vzMax)/2.0/uzd; - zfU = zfU - zDelta; - vz0U = vzMax; - az = -uzd; - } - else if ( vz0U < 0 ){ - // 需要反向，先把原来的速度减到零 - PRINT_MSG("Case 1"); - t = fabs(vz0U/uzd); - zDelta = vz0U*t + uzd*t*t/2.0; - z0U = 0; - zfU = zfU-zDelta; - vz0U = 0; // vz0 + uzd*t; // "correct" solution isn't great, rounding errors - az = uzd; - } - else if ( vz0U > vzf && zfU <= ((vz0U*vz0U - vzf*vzf)/2.0/uzd + tol_pos) ){ - // 从现在起以最大减速度减速，也会超过目标点 - // Case 2.3: Need to decelerate - PRINT_MSG("Case 2.3"); - zDelta = (vz0U*vz0U - vzf*vzf)/2.0/uzd; - t = (vz0U-vzf)/uzd; - zfU = zfU-zDelta; - vz0U = vzf; - z0U = 0; - az = -uzd; - } - else if ( fabs(vz0U - vzMax) < tol_vel ){ - // 计算保持最大速度的时间，这段时间运动的距离，及到减速时剩下的路程 - //Case 2.2: Cruising at vzMax until you have to decelerate - PRINT_MSG("Case 2.2"); - z0U = 0; - zDelta = (vzMax*vzMax - vzf*vzf)/2.0/uzd; - vz0U = vzMax; - az = 0; - double zCruise = zfU - zDelta; - t = zCruise / vzMax; - zfU = (vzMax*vzMax - vzf*vzf)/2.0/uzd; - } - else{ - // Case 2.1: Accelerate - PRINT_MSG("Case 2.1"); - // Accelerate to vzMax - double t1 = (vzMax - vz0U)/uza; - - // Accelerate without reaching vzMax - zDelta = vz0U*vz0U/2.0/uza + vzf*vzf/2.0/uzd; - float temp = (float)( (zfU + zDelta)/(1.0/2/uza + 1.0/2/uzd) ); - double vz2 = sqrt( temp ); - double t2 = (vz2 - vz0U)/uza; - if (t1 storeData(data,data+6); - DataVector.push_back(storeData); - t0U = t + t0U; //Initial time for the next stage - - if ( vz0U == vzMax && vzMax < 0 ) - break; - } - //std::cout<<"~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~numOfStep "< storeData(data,data+6); - DataVector.push_back(storeData); - return DataVector; -} - -vector< vector > tg_recurs(double t0, - double z0, - double zf, - double vz0, - double vzf, - int rev, - int iter, - double uza, - double uzd, - double vzMax, - double tol_pos, - double tol_vel) -{ - const double tol = 0.01; // tolerance for machine precision - //const double tol_pos = 1; //pos_error - //const double tol_vel = 2; //vel_error - double zfU = zf; - double z0U = 0; - double vz0U = vz0; - double az; - double t=0; - double t0U = 0; - double zfU2; - - vector > DataVector; - DataVector.clear(); - iter = iter + 1; -// int numOfStep = 0; // -// bool canReach = true; -// double zDelta = 0; - - bool isGotoBehindFirst = false; - - if (fabs(z0)!=0) - { - // Adjust offset for z0 - zfU = zf-z0; - z0U = 0; - vz0U = vz0; - az = 0; - t = 0; - } - /// 以位移的方向为正 - if (zfU<0 ) - { - // Destination in wrong direction, need to flip the sign - // display('Flip direction'); - z0U = 0; - zfU = -zfU; - vz0U = -vz0U; - vzf = -vzf; - az = 0; - t = 0; - rev = rev == 1 ? -1 : 1; - - double data[6] = { z0U, vz0, az, rev, t, t0U }; - vector storeData(data, data + 6 ); - DataVector.push_back( storeData ); - t0U = t + t0U; //Initial time for the next stage - } - - int revold = rev; - double vf = vzf; - double zMinDist1, zMinDist2; - - /// 达到末速度的最短距离 - zMinDist1 = (vf*vf)/2/uza; - /// 末速度大于 0 - if ( vf > 0 ){ - if ( vz0U > 0 ) - { /// 初速度大于 0 - double d = (vf*vf-vz0U*vz0U); - /// 末速度与初速度的关系 - /// 如果初速度比末速度小，则 zMinDist2 等于最短加速距离 - /// 如果初速度比末速度大，则 zMinDist2 等于最短减速距离 - zMinDist2 = vf > vz0U ? d/2/uza : -d/2/uzd; - } - else{ - /// 初速度小于 0 - zMinDist2 = (vf*vf)/2/uza - (vz0U*vz0U)/2/uzd; - } - /// 如果达到末速度的所需的最短加速距离小于 zfU - /// 机器人需先后退到 zfU - zMinDist1， 然后再加速 - /// - if ( zMinDist2 > zfU ) - { - zfU2 = zfU - zMinDist1; - genTrajNone(DataVector, - zfU2, - vz0U, - 0.0, - rev, - uza, - uzd, - vzMax, - tol_pos, - tol_vel); - zfU = zMinDist1; - vz0U = 0.0; - rev = revold; - isGotoBehindFirst = true; - // std::cout<<" backup first 1 ~~~~~~~~~~~~~~~"< 0 ) - { /// 初速大于零 - zMinDist2 = zfU-vz0U*vz0U/2/uzd; - if (zMinDist2 < -zMinDist1){ - zfU2 = vz0U*vz0U/2/uzd; - zfU = zMinDist2; - } - else{ - zfU2 = zfU+zMinDist1; - zfU = -zMinDist1; - } - } - else{ ///初速度小于零 - zMinDist2 = zfU+vz0U*vz0U/2/uzd; - zfU2 = zfU+zMinDist1; - zfU = -zMinDist1; - } - - genTrajNone(DataVector, - zfU2, - vz0U, - 0.0, - rev, - uza, - uzd, - vzMax, - tol_pos, - tol_vel); - vz0U = 0.0; - - rev = revold; - isGotoBehindFirst = true; - //std::cout<<" backup first 2 ~~~~~~~~~~~~~~~"<>& storeData,double& z, double& vz, -/// double& azList) -/// -/// @brief 根据函数 的结果返回下一周期的位移 z，速度 vz 和加速度 azList。 -/// -/// 1. 根据算法的基本假设，加速度恒定，因此加速度 a = storeData[stage-1][2]; -/// 2. 速度 v = v0 + a*dt -/// 3. 位移 z= z0 + v*dt -/// -/// @author Yonghai Wu -/// @date 2009-12-9 -/// -/// @param storeData Information describing the pathe. -/// @param [in,out] z the next postion. -/// @param [in,out] vz the next velocity. -/// @param [in,out] azList the next acceration. -//////////////////////////////////////////////////////////////////////////////////////////////////// - -void buildPathNone(const vector>& storeData,double& z, double& vz, double& azList) -{ - double num_stages = storeData.size(); - vector data = storeData[num_stages-1]; - double Duration = data[5]; // 运动控制总时间 - int stage = 0; - z = storeData[0][0]; - int tfint = Duration*PARAM::Vision::FRAME_RATE; - int maxStepNumber=(std::min)(tfint, 1); - for (int index = 0;index<=maxStepNumber;index++){ - if (index>=Duration*PARAM::Vision::FRAME_RATE) - continue; - while (index>=storeData[stage][5]*PARAM::Vision::FRAME_RATE) - stage = stage + 1; - double t0 = storeData[stage-1][5] ; - double vz0 = storeData[stage-1][1]; - double az = storeData[stage-1][2]; - double rev = storeData[stage-1][3]; - azList = rev*az; - vz = rev*vz0 + rev*az * (index*1.0/PARAM::Vision::FRAME_RATE-t0); - if (index!=0) - z+=vz0/PARAM::Vision::FRAME_RATE; - } - // This is to prevent errors when duration = 0 - if (tfint == 0){ - //std::cout<<"duration = 0!!!!!"< >& storeData, double& z, double& vz, -/// double& azList, vector< vector >& pathList) -/// -/// 根据函数 的结果返回下一周期的位移 z，速度 vz 和加速度 azList 和整条路径 pathList。 -/// -/// 1. 根据算法的基本假设，加速度恒定，因此加速度 a = storeData[stage-1][2]; -/// 2. 速度 v(t) = v0 + a*dt -/// 3. 位移 z(t) = z0 + v(t)*dt -/// -/// @author Yonghai Wu -/// @date 2009-12-9 -/// -/// @param storeData Information describing the store. -/// @param [in,out] z the z coordinate. -/// @param [in,out] vz the vz. -/// @param [in,out] azList list of azs. -/// @param [in,out] pathList list of pathes. -//////////////////////////////////////////////////////////////////////////////////////////////////// - -void buildPathNone(const vector< vector >& storeData, - double& z, - double& vz, - double& azList, - vector< vector >& pathList) -{ - double num_stages = storeData.size(); - vector data = storeData[num_stages-1]; - double Duration = data[5]; // 运动控制总时间 - - if ( abs(Duration) < 0.00001 ) - { - double data[3] = { z, vz, azList }; - vector storedata(data, data+3); - pathList.push_back(storedata); - return; - } - int stage = 0; - double z_t, vz_t, azList_t; - z_t = storeData[0][0]; - int tfint = Duration*PARAM::Vision::FRAME_RATE; - int maxStepNumber = /*(std::max)(tfint, 1) */(std::min)( 1000, (std::max)(tfint, 1)); - int indexStep = 3; - for (int index = -1;index<=maxStepNumber;){ - if( index < 1 ) - index ++; - else - index += indexStep; - - if (index>=Duration*PARAM::Vision::FRAME_RATE) - continue; - while (index>=storeData[stage][5]*PARAM::Vision::FRAME_RATE) - stage = stage + 1; - double t0 = storeData[stage-1][5]; - double vz0 = storeData[stage-1][1]; - double az = storeData[stage-1][2]; - double rev = storeData[stage-1][3]; - azList_t = rev*az; - vz_t = rev*vz0 + rev*az * (index*1.0/PARAM::Vision::FRAME_RATE-t0); - if (index!=0 ){ - if( index == 1 ) - z_t+=vz_t/PARAM::Vision::FRAME_RATE; - else - z_t+=indexStep*vz_t/PARAM::Vision::FRAME_RATE; - } - - if ( 1 == index) - { - z = z_t; - vz = vz_t; - azList = azList_t; - //std::cout<<"stage:"< storedata(data, data+3); - pathList.push_back(storedata); - // std::cout<<"stage:"< > storeDataX = tg_recurs(t0, x0, xf, vx0, 1,rev, iter, axMax, factor*axMax, vxMax); - double num_stagesX = storeDataX.size(); - durX = storeDataX[num_stagesX-1][5]; - //Find duration for y-direction - iter = 0; - t0 = 0; - rev = 1; - vector > storeDataY = tg_recurs(t0, y0, yf, vy0, 1,rev, iter, ayMax, factor*ayMax, vyMax); - double num_stagesY = storeDataY.size(); - durY = storeDataY[num_stagesY-1][5]; - double tfDelta = durX-durY; - //std::cout<<"alpha:"< >& pathList) -/// -/// @brief 非零速到点路径规划函数。算法流程如下： -/// 1. 采用二分法，同步 x 和 y 方向的位移 -/// 2. 独立计算转动方向的所化时间 t_theta, 取 t_x, t_y, t_theta 中的最大值 -/// 为最后所花的时间 -/// -/// @author Yonghai Wu -/// @date 2010-3-24 -/// -/// @param start The start. -/// @param final The final. -/// @param frameRate The frame rate. -/// @param capability The capability. -/// @param [in,out] nextStep the next step. 下一周期的位移和速度 -/// @param [in,out] pathList list of pathes. 整条路径 -//////////////////////////////////////////////////////////////////////////////////////////////////// - -void nonetrapezoidalVelocityPath( const PlayerPoseT& start, - const PlayerPoseT& final, - double frameRate, - const PlayerCapabilityT& capability, - PlayerPoseT& nextStep, - vector< vector >& pathList) - { - - - double x0 = start.X(); - double y0 = start.Y(); - double xf = final.X(); - double yf = final.Y(); - double vx0 = start.VelX(); - double vy0 = start.VelY(); - double vxf = final.VelX(); - double vyf = final.VelY(); - double vtheta0 = start.RotVel(); - double vthetaf = final.RotVel(); - double theta0 = start.Dir(); - double thetaf = final.Dir(); - - double x = x0,vx = vx0 ,ax = 0,y = y0,vy = vy0,ay=0,theta = theta0,vtheta = vtheta0,atheta = 0; - if (capability.maxAccel==0 - || capability.maxAngularAccel==0 - || capability.maxSpeed==0 - || capability.maxAngularSpeed==0){ - cout << "trapezoidalVelocityPath code error: invalid scale(s)" << endl; - } - /*std::cout<<" maxacc: "< PARAM::Math::PI) - dtheta = dtheta - 2*PARAM::Math::PI; - thetaf=theta0 + dtheta; - - //double controlAlpha = - // syncTG(x0,y0,vx0,vy0,maxAccel,maxAccel,maxSpeed); Do a binary - // search to find the proper control effort, such that the times - // for the trajectories in x and y direction are the same. The - // variable to vary is "alpha", which runs from 0 to pi/2 - - // syncTG(start, final, capability); - - - int numIter = 10; //number of iterations performed. error in u is 2^(-numIter) - double u = 1.568; // approx. PI/2 - double du = -1.568; - double alpha,durX,durY; - vector > storeDataX,storeDataY; - int iter = 0; - double t0 = 0; - double rev = 1; - float normOfFinalSpeed = sqrt(vxf*vxf + vyf*vyf); - if ( normOfFinalSpeed > maxSpeed*0.9) - { - //std::cout<<"nonetrap: final speed is too big"< > storeDataTheta = tg_recurs(t0, - theta0, - thetaf, - vtheta0, - vthetaf, - rev, - iter, - maxAngleAccel, - maxAngleDec, - maxAngleSpeed); - - vector< vector > pathListR, pathListX, pathListY; - buildPathNone(storeDataTheta,theta,vtheta,atheta,pathListR); - buildPathNone(storeDataX,x,vx,ax, pathListX); - buildPathNone(storeDataY,y,vy,ay, pathListY); - - -// double duration = durX > durY ? durX : durY; // 运动控制总时间 - - int pathStepX = pathListX.size(); - int pathStepY = pathListY.size(); - int pathStepR = pathListR.size(); - //std::cout<<" path len: r "<= pathStepX) - { - if ( pathStepX > 0) - { - data[4] = pathListX[pathStepX-1][1]; - data[1] = pathListX[pathStepX-1][0]+(i-pathStepX+1)*data[4]/PARAM::Vision::FRAME_RATE; - } - else - { - data[4] = vx0; - data[1] = 0; - } - - } - else{ - data[1] = pathListX[i][0]; - data[4] = pathListX[i][1]; - } - if ( i >= pathStepY) - { - if ( pathStepY > 0 ) - { - data[5] = pathListY[pathStepY-1][1]; - data[2] = pathListY[pathStepY-1][0]+(i-pathStepY+1)*data[5]/PARAM::Vision::FRAME_RATE; - } - else - { - data[2] = 0; - data[5] = vy0; - } - - } - else{ - data[2] = pathListY[i][0]; - data[5] = pathListY[i][1]; - } - - if ( i >= pathStepR) - { - if ( pathStepR > 0) - { - data[6] = pathListR[pathStepR-1][1]; - data[3] = pathListR[pathStepR-1][0]+(i-pathStepR+1)*data[6]/PARAM::Vision::FRAME_RATE; - } - else - { - data[3] = 0; - data[6] = vtheta0; - } - - } - else{ - data[3] = pathListR[i][0]; - data[6] = pathListR[i][1]; - } - vector storeData(data, data+7); - pathList.push_back(storeData); - // PRINT_DEBUG(pathList.size()); - } - } diff --git a/Core/src/MotionControl/noneTrapzodalVelTrajectory.h b/Core/src/MotionControl/noneTrapzodalVelTrajectory.h deleted file mode 100644 index d7a4113..0000000 --- a/Core/src/MotionControl/noneTrapzodalVelTrajectory.h +++ /dev/null @@ -1,89 +0,0 @@ -////////////////////////////////////////////////////////////////////// -// @file: algorithm\noneTrapzodalVelTrajectory.h -// @brief: 非零速到点轨迹规划算法 -// 基于运动学分析的运动控制方法。其算法的核心思想如下： -// 该方法假设机器人在加速和减速过程中，机器人的加速度是恒定不变的。 -// 在这个假设的基础上，根据机器人的初始状态, 将机器人的运动分为三 -// 大类： -// 1: 初速度与位移方向不一致，先将减速到 0 -// 2: 初速度与位移方向一致 -// 2.1: 初速度小于最大速度 Vmax, 且加速至最大速度时位移不会 -// 超过目标点，则先加速至最大速度。 -// 2.2: 保持最大速度运动 -// 2.3: 以最大减速度减速，也会超过目标点，则以最大减速减速 -// 3: 初速大于最大速度，先减速到 Vmax -// -// 参考文献: -// Trajectory generation and control for four wheeled -// omnidirectional vehicles. - -/// Oliver Purwin, Raffaello D'Andrea. Robotics and -// Autonomous Systems 54 (2006) 13–22 -// summary: Declares the nonetrapzodal vel trajectory class -///////////////////////////////////////////////////////////////////// - -#ifndef NONE_TRAPEZOIDAL_VEL_TRAJECTORY_H -#define NONE_TRAPEZOIDAL_VEL_TRAJECTORY_H -#include -#include - - -using namespace std; - -vector> genTrajZero(vector> &DataVector, - double zfU, - double vz0, - int rev, - double uza, - double uzd, - double vzMax, - double tol_pos, - double tol_vel); - -vector> genTrajNone(vector> &DataVector, - double zfU, - double vz0, - double vzf, - int rev, - double uza, - double uzd, - double vzMax, - double tol_pos, - double tol_vel); - -vector> tg_recurs(double t0, - double z0, - double zf, - double vz0, - double vzf, - int rev, - int iter, - double uza, - double uzd, - double vzMax); - -void buildPathNone(const vector< vector > & storeData, - double& z, - double& vz, - double& azList, - vector< vector >& pathList); - -void buildPathNone(const vector> & storeData, - double& z, - double& vz, - double& azList); - -double syncTGNone(const PlayerPoseT& start, - CGeoPoint finalPos, - const PlayerCapabilityT& capability); - - - -void nonetrapezoidalVelocityPath( const PlayerPoseT& start, - const PlayerPoseT& final, - double frameRate, - const PlayerCapabilityT& capability, - PlayerPoseT& nextStep, - vector< vector >& pathList - ); -#endif