81 lines
1.9 KiB
C++
81 lines
1.9 KiB
C++
#ifndef LOWLEVELCMD_H
|
|
#define LOWLEVELCMD_H
|
|
|
|
#include "math/mathTypes.h"
|
|
#include <vector>
|
|
#include <iostream>
|
|
|
|
struct LowlevelCmd{
|
|
private:
|
|
size_t _dof = 6;
|
|
public:
|
|
Vec6 endPosture;
|
|
std::vector<double> q;
|
|
std::vector<double> dq;
|
|
std::vector<double> tau;
|
|
std::vector<double> kp;
|
|
std::vector<double> kd;
|
|
Vec6 posture;
|
|
|
|
LowlevelCmd();
|
|
~LowlevelCmd(){}
|
|
/* dq = {0} */
|
|
void setZeroDq();
|
|
/* tau = {0} */
|
|
void setZeroTau();
|
|
/* kp = {0} */
|
|
void setZeroKp();
|
|
/* kd = {0} */
|
|
void setZeroKd();
|
|
/*
|
|
* set default arm kp & kd (Only used in State_LOWCMD)
|
|
* kp = [20, 30, 30, 20, 15, 10]
|
|
* kd = [2000, 2000, 2000, 2000, 2000, 2000]
|
|
*/
|
|
void setControlGain();
|
|
/* kp = KP, kd = KW */
|
|
void setControlGain(std::vector<double> KP, std::vector<double> KW);
|
|
/* q = qInput */
|
|
void setQ(std::vector<double> qInput);
|
|
/* q = qInput */
|
|
void setQ(VecX qInput);
|
|
/* dq = qDInput */
|
|
void setQd(VecX qDInput);
|
|
/* tau = tauInput */
|
|
void setTau(VecX tauInput);
|
|
/*
|
|
* setZeroDq()
|
|
* setZeroTau()
|
|
* setZeroKp()
|
|
* kd = [10, 10, 10, 10, 10, 10]
|
|
*/
|
|
void setPassive();
|
|
/*
|
|
* set default gripper kp & kd (Only used in State_LOWCMD)
|
|
* kp.at(kp.size()-1) = 20
|
|
* kd.at(kd.size()-1) = 2000
|
|
*/
|
|
void setGripperGain();
|
|
/*
|
|
* kp.at(kp.size()-1) = KP
|
|
* kd.at(kd.size()-1) = KW
|
|
*/
|
|
void setGripperGain(double KP, double KW);
|
|
/* set gripper kp&kd to zero */
|
|
void setGripperZeroGain();
|
|
/* q.at(q.size()-1) = qInput; */
|
|
void setGripperQ(double qInput);
|
|
double getGripperQ() {return q.at(q.size()-1);}
|
|
void setGripperQd(double qdInput) {dq.at(dq.size()-1) = qdInput;}
|
|
double getGripperQd() {return dq.at(dq.size()-1);}
|
|
void setGripperTau(double tauInput) {tau.at(tau.size()-1) = tauInput;}
|
|
double getGripperTau() {return tau.at(tau.size()-1);}
|
|
/* return Vec6 from std::vector<double> q, without gripper */
|
|
Vec6 getQ();
|
|
/* return Vec6 from std::vector<double> dq, without gripper */
|
|
Vec6 getQd();
|
|
};
|
|
|
|
|
|
#endif //LOWLEVELCMD_H
|