50 lines
1.1 KiB
C++
Executable File
50 lines
1.1 KiB
C++
Executable File
#ifndef FSMSTATE_H
|
|
#define FSMSTATE_H
|
|
|
|
#include <string>
|
|
#include <iostream>
|
|
#include <unistd.h>
|
|
#include "control/CtrlComponents.h"
|
|
#include "common/math/mathTools.h"
|
|
#include "common/utilities/timer.h"
|
|
#include "FSM/BaseState.h"
|
|
|
|
class FSMState : public BaseState{
|
|
public:
|
|
FSMState(CtrlComponents *ctrlComp, ArmFSMStateName stateName, std::string stateNameString);
|
|
virtual ~FSMState(){}
|
|
|
|
virtual void enter() = 0;
|
|
virtual void run() = 0;
|
|
virtual void exit() = 0;
|
|
virtual int checkChange(int cmd) {return (int)ArmFSMStateName::INVALID;}
|
|
bool _collisionTest();
|
|
|
|
protected:
|
|
void _armCtrl();
|
|
void _recordData();
|
|
Vec6 _postureToVec6(Posture posture);
|
|
void _tauFriction();
|
|
|
|
LowlevelCmd *_lowCmd;
|
|
LowlevelState *_lowState;
|
|
IOInterface *_ioInter;
|
|
ArmModel *_armModel;
|
|
|
|
Vec6 _qPast, _qdPast, _q, _qd, _qdd, _tauForward;
|
|
double _gripperPos, _gripperW, _gripperTau;
|
|
|
|
CtrlComponents *_ctrlComp;
|
|
Vec6 _g, _tauCmd, _tauFric;
|
|
|
|
private:
|
|
|
|
uint _collisionCnt;
|
|
|
|
Vec6 _mLinearFriction;
|
|
Vec6 _mCoulombFriction;
|
|
|
|
};
|
|
|
|
#endif // FSMSTATE_H
|