168 lines
3.5 KiB
C++
Executable File
168 lines
3.5 KiB
C++
Executable File
#ifndef LOWLEVELSTATE_HPP
|
|
#define LOWLEVELSTATE_HPP
|
|
|
|
#include <iostream>
|
|
#include <vector>
|
|
#include "common/math/mathTypes.h"
|
|
#include "common/math/mathTools.h"
|
|
#include "common/enumClass.h"
|
|
#include "common/utilities/LPFilter.h"
|
|
#include "message/UserValue.h"
|
|
|
|
// struct MotorState
|
|
// {
|
|
// unsigned int mode;
|
|
// float q;
|
|
// float dq;
|
|
// float ddq;
|
|
// float tauEst;
|
|
|
|
// MotorState(){
|
|
// q = 0;
|
|
// dq = 0;
|
|
// ddq = 0;
|
|
// tauEst = 0;
|
|
// }
|
|
// };
|
|
|
|
struct LowlevelState{
|
|
private:
|
|
size_t _dof = 6;
|
|
public:
|
|
LowlevelState(double dt){
|
|
#ifndef UNITREE_GRIPPER
|
|
qFilter = new LPFilter(dt, 3.0, _dof);
|
|
dqFilter = new LPFilter(dt, 3.0, _dof);
|
|
tauFilter = new LPFilter(dt, 5.0, _dof);
|
|
|
|
q.resize(_dof);
|
|
dq.resize(_dof);
|
|
ddq.resize(_dof);
|
|
tau.resize(_dof);
|
|
qFiltered.resize(_dof);
|
|
dqFiltered.resize(_dof);
|
|
tauFiltered.resize(_dof);
|
|
#endif
|
|
#ifdef UNITREE_GRIPPER
|
|
qFilter = new LPFilter(dt, 3.0, _dof+1);
|
|
dqFilter = new LPFilter(dt, 3.0, _dof+1);
|
|
tauFilter = new LPFilter(dt, 5.0, _dof+1);
|
|
|
|
q.resize(_dof+1);
|
|
dq.resize(_dof+1);
|
|
ddq.resize(_dof+1);
|
|
tau.resize(_dof+1);
|
|
qFiltered.resize(_dof+1);
|
|
dqFiltered.resize(_dof+1);
|
|
tauFiltered.resize(_dof+1);
|
|
#endif
|
|
}
|
|
~LowlevelState(){
|
|
delete qFilter;
|
|
delete dqFilter;
|
|
delete tauFilter;
|
|
}
|
|
|
|
// MotorState motorState[12];
|
|
std::vector<double> q;
|
|
std::vector<double> dq;
|
|
std::vector<double> ddq;
|
|
std::vector<double> tau;
|
|
|
|
std::vector<double> qFiltered;
|
|
std::vector<double> dqFiltered;
|
|
std::vector<double> tauFiltered;
|
|
|
|
// ArmFSMStateName userCmd;
|
|
UserValue userValue;
|
|
LPFilter *qFilter;
|
|
LPFilter *dqFilter;
|
|
LPFilter *tauFilter;
|
|
|
|
void runFilter(){
|
|
qFiltered = q;
|
|
qFilter->addValue(qFiltered);
|
|
|
|
dqFiltered = dq;
|
|
dqFilter->addValue(dqFiltered);
|
|
|
|
tauFiltered = tau;
|
|
tauFilter->addValue(tauFiltered);
|
|
}
|
|
|
|
Vec6 getQ(){
|
|
Vec6 qReturn;
|
|
for(int i(0); i < 6; ++i){
|
|
qReturn(i) = q.at(i);
|
|
}
|
|
return qReturn;
|
|
}
|
|
|
|
Vec6 getQd(){
|
|
Vec6 qdReturn;
|
|
for(int i(0); i < 6; ++i){
|
|
qdReturn(i) = dq.at(i);
|
|
}
|
|
return qdReturn;
|
|
}
|
|
|
|
Vec6 getQdd(){
|
|
Vec6 qddReturn;
|
|
for(int i(0); i < 6; ++i){
|
|
qddReturn(i) = ddq.at(i);
|
|
}
|
|
return qddReturn;
|
|
}
|
|
|
|
Vec6 getTau(){
|
|
Vec6 tauReturn;
|
|
for(int i(0); i < 6; ++i){
|
|
tauReturn(i) = tau.at(i);
|
|
}
|
|
return tauReturn;
|
|
}
|
|
|
|
Vec6 getQFiltered(){
|
|
Vec6 qReturn;
|
|
for(int i(0); i < 6; ++i){
|
|
qReturn(i) = qFiltered.at(i);
|
|
}
|
|
return qReturn;
|
|
}
|
|
|
|
Vec6 getQdFiltered(){
|
|
Vec6 dqReturn;
|
|
for(int i(0); i < 6; ++i){
|
|
dqReturn(i) = dqFiltered.at(i);
|
|
}
|
|
return dqReturn;
|
|
}
|
|
|
|
|
|
Vec6 getTauFiltered(){
|
|
Vec6 tauReturn;
|
|
for(int i(0); i < 6; ++i){
|
|
tauReturn(i) = tauFiltered.at(i);
|
|
}
|
|
return tauReturn;
|
|
}
|
|
|
|
double getGripperQ(){
|
|
return q.at(q.size()-1);
|
|
}
|
|
|
|
double getGripperQd(){
|
|
return dq.at(q.size()-1);
|
|
}
|
|
|
|
double getGripperTau(){
|
|
return tau.at(tau.size()-1);
|
|
}
|
|
|
|
double getGripperTauFiltered(){
|
|
return tauFiltered.at(tau.size()-1);
|
|
}
|
|
};
|
|
|
|
#endif //LOWLEVELSTATE_HPP
|