Skip to content

Commit

Permalink
[Core] delete unused traj plan
Browse files Browse the repository at this point in the history
  • Loading branch information
@Mark-tz
Mark-tz committed Apr 17, 2024
1 parent 6f5bb02 commit 7e52676
Show file tree
Hide file tree
Showing 6 changed files with 0 additions and 2,278 deletions.
120 changes: 0 additions & 120 deletions Core/src/MotionControl/ControlModel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,6 @@
#include <cornell/Trajectory.h> // for Cornell BangBang
#include <cornell/TrajectorySupport.h>

#include <TrapezoidalVelTrajectory.h>
#include <noneTrapzodalVelTrajectory.h>
#include <CMmotion.h>
#include <parammanager.h>

Expand Down Expand Up @@ -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<double> 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<double> 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 ;
//}

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

Expand Down
9 changes: 0 additions & 9 deletions Core/src/MotionControl/ControlModel.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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; }

Expand Down
Loading

0 comments on commit 7e52676

Please sign in to comment.