2022-07-20 11:11:38 +08:00
|
|
|
#ifndef CARTESIANPATH_H
|
|
|
|
|
#define CARTESIANPATH_H
|
|
|
|
|
|
|
|
|
|
#include "FSM/FSMState.h"
|
|
|
|
|
#include "trajectory/TrajectoryManager.h"
|
|
|
|
|
|
|
|
|
|
class State_Trajectory : public FSMState{
|
|
|
|
|
public:
|
|
|
|
|
State_Trajectory(CtrlComponents *ctrlComp,
|
|
|
|
|
ArmFSMStateName stateEnum = ArmFSMStateName::TRAJECTORY,
|
|
|
|
|
std::string stateString = "trajectory");
|
|
|
|
|
~State_Trajectory();
|
|
|
|
|
void enter();
|
|
|
|
|
void run();
|
2022-09-13 19:53:15 +08:00
|
|
|
void exit();
|
|
|
|
|
int checkChange(int cmd);
|
2022-07-20 11:11:38 +08:00
|
|
|
protected:
|
2022-11-11 19:49:41 +08:00
|
|
|
void _setTraj();
|
2023-03-02 15:00:21 +08:00
|
|
|
|
2022-07-20 11:11:38 +08:00
|
|
|
TrajectoryManager *_traj;
|
|
|
|
|
HomoMat _goalHomo;
|
|
|
|
|
Vec6 _goalTwist;
|
2022-11-11 19:49:41 +08:00
|
|
|
double speedTemp;
|
2022-07-20 11:11:38 +08:00
|
|
|
|
|
|
|
|
JointSpaceTraj *_toStartTraj;
|
|
|
|
|
bool _reachedStart = false;
|
|
|
|
|
bool _finishedTraj = false;
|
|
|
|
|
std::vector<JointSpaceTraj*> _jointTraj;
|
|
|
|
|
std::vector<EndCircleTraj*> _circleTraj;
|
|
|
|
|
std::vector<StopForTime*> _stopTraj;
|
|
|
|
|
std::vector<EndLineTraj*> _lineTraj;
|
|
|
|
|
|
2022-09-13 19:53:15 +08:00
|
|
|
static std::vector<TrajCmd> _trajCmd;
|
|
|
|
|
Vec6 _posture[2];
|
2022-07-20 11:11:38 +08:00
|
|
|
};
|
|
|
|
|
|
|
|
|
|
#endif // CARTESIANPATH_H
|
