2022-11-11 18:17:07 +08:00
|
|
|
#ifndef CTRLCOMPONENTS_H
|
|
|
|
#define CTRLCOMPONENTS_H
|
|
|
|
|
2022-12-05 12:17:23 +08:00
|
|
|
#include "message/arm_common.h"
|
|
|
|
#include "message/LowlevelCmd.h"
|
|
|
|
#include "message/LowlevelState.h"
|
|
|
|
#include "message/udp.h"
|
|
|
|
#include "utilities/loop.h"
|
|
|
|
#include "model/ArmModel.h"
|
2022-11-11 18:17:07 +08:00
|
|
|
|
2022-11-16 16:23:36 +08:00
|
|
|
namespace UNITREE_ARM {
|
2022-11-11 18:17:07 +08:00
|
|
|
struct CtrlComponents{
|
|
|
|
public:
|
2022-11-15 16:05:20 +08:00
|
|
|
CtrlComponents(double deltaT, bool hasUnitreeGripper);
|
2022-11-11 18:17:07 +08:00
|
|
|
~CtrlComponents();
|
2022-11-15 16:05:20 +08:00
|
|
|
/*
|
|
|
|
* Function: send udp message to z1_ctrl and receive udp message from it
|
|
|
|
* Input: None
|
|
|
|
* Output: None
|
|
|
|
* Description: The function will call udp->send() to send datas in lowcmd to z1_ctrl
|
|
|
|
* and call udp->recv() to store datas from z1_ctrl into lowstate
|
|
|
|
*/
|
2022-11-11 18:17:07 +08:00
|
|
|
void sendRecv();
|
2022-11-15 16:05:20 +08:00
|
|
|
/*
|
|
|
|
* Function: Set six joints commands to class lowcmd
|
|
|
|
* Input: q: joint angle
|
|
|
|
* qd: joint velocity
|
|
|
|
* tau: joint (Only used in State_LOWCMD)
|
|
|
|
* Output: None
|
|
|
|
*/
|
2022-11-11 18:17:07 +08:00
|
|
|
void armCtrl(Vec6 q, Vec6 qd, Vec6 tau);
|
2022-11-15 16:05:20 +08:00
|
|
|
/*
|
|
|
|
* Function: Set gripper commands to class lowcmd
|
|
|
|
* Input: q: joint angle
|
|
|
|
* qd: joint velocity
|
|
|
|
* tau: joint (Only used in State_LOWCMD)
|
|
|
|
* Output: None
|
|
|
|
*/
|
2022-11-11 18:17:07 +08:00
|
|
|
void gripperCtrl(double gripperPos, double gripperW, double gripperTau);
|
|
|
|
|
|
|
|
LowlevelCmd *lowcmd;
|
|
|
|
LowlevelState *lowstate;
|
2022-11-15 16:05:20 +08:00
|
|
|
double dt;// default: 0.002
|
|
|
|
SendCmd sendCmd; // udp command to control the arm
|
2022-11-11 18:17:07 +08:00
|
|
|
RecvState recvState; // the arm state receive from udp
|
|
|
|
ArmFSMState statePast;
|
2022-11-15 16:05:20 +08:00
|
|
|
ArmModel* armModel;
|
2022-11-11 18:17:07 +08:00
|
|
|
private:
|
|
|
|
UDPPort *_udp;
|
|
|
|
};
|
|
|
|
|
2022-11-16 16:23:36 +08:00
|
|
|
}
|
2022-11-11 18:17:07 +08:00
|
|
|
#endif
|