first commit
This commit is contained in:
David Zhai
commit
5221e95827
|
|
@ -0,0 +1 @@
|
|||
build
|
||||
|
|
@ -0,0 +1,35 @@
|
|||
cmake_minimum_required(VERSION 3.0)
|
||||
project(z1_sdk)
|
||||
|
||||
# set(EIGEN_PATH /usr/include/eigen3)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
# ${EIGEN_PATH}
|
||||
)
|
||||
|
||||
link_directories(lib)
|
||||
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3 -pthread")
|
||||
|
||||
# file(GLOB_RECURSE ADD_SRC_LIST
|
||||
# src/*.cpp
|
||||
# )
|
||||
|
||||
# set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/../lib) # SHARED:CMAKE_LIBRARY_OUTPUT_DIRECTORY STATIC:CMAKE_ARCHIVE_OUTPUT_DIRECTORY
|
||||
# add_library(Z1_SDK_Linux64 SHARED ${ADD_SRC_LIST})
|
||||
|
||||
add_executable(example_lowCmd_send examples/example_lowCmd_send.cpp)
|
||||
target_link_libraries(example_lowCmd_send Z1_SDK_Linux64)
|
||||
|
||||
add_executable(example_state_send examples/example_state_send.cpp)
|
||||
target_link_libraries(example_state_send Z1_SDK_Linux64)
|
||||
|
||||
add_executable(example_state_recv examples/example_state_recv.cpp)
|
||||
target_link_libraries(example_state_recv Z1_SDK_Linux64)
|
||||
|
||||
add_executable(example_keyboard_recv examples/example_keyboard_recv.cpp)
|
||||
target_link_libraries(example_keyboard_recv Z1_SDK_Linux64)
|
||||
|
||||
add_executable(example_keyboard_send examples/example_keyboard_send.cpp)
|
||||
target_link_libraries(example_keyboard_send Z1_SDK_Linux64)
|
||||
|
|
@ -0,0 +1,76 @@
|
|||
# v3.6.1
|
||||
The z1_sdk is mainly used for communication between z1_controller and command panel(keyboard, joystick, user control panel etc).
|
||||
|
||||
## Notice
|
||||
|
||||
support robot: Z1(v3.6.1)
|
||||
not support robot: Z1(v3.4, v3.5)
|
||||
|
||||
## Dependencies
|
||||
|
||||
- build-essential
|
||||
|
||||
```bash
|
||||
sudo apt install build-essential
|
||||
```
|
||||
|
||||
- Boost (version 1.5.4 or higher)
|
||||
|
||||
```bash
|
||||
dpkg -S /usr/include/boost/version.hpp # check boost version
|
||||
sudo apt install libboost-dev # install boost
|
||||
```
|
||||
|
||||
- CMake (version 2.8.3 or higher)
|
||||
|
||||
```bash
|
||||
cmake --version # check cmake version
|
||||
sudo apt install cmake # install cmake
|
||||
```
|
||||
|
||||
- [Eigen](https://gitlab.com/libeigen/eigen/-/releases/3.3.9) (version 3.3.9 or higher)
|
||||
|
||||
```bash
|
||||
cd eigen-3.3.9
|
||||
mkdir build && cd build
|
||||
cmake ..
|
||||
sudo make install
|
||||
sudo ln -s /usr/local/include/eigen3 /usr/include/eigen3
|
||||
sudo ln -s /usr/local/include/eigen3/Eigen /usr/local/include/Eigen
|
||||
```
|
||||
|
||||
## Example
|
||||
|
||||
There are three examples of command panel control based on SDK in this project, you can use it fllowing the steps below.
|
||||
|
||||
### State change
|
||||
|
||||
- First, set(CTRL_PANEL SDK) # z1_ws/src/z1_controller/CMakeList.txt，and then rebuild the z1_ws to generate z1_ctrl, then open a teminal to run z1_ctrl
|
||||
|
||||
```
|
||||
cd /<path to>/z1_controller/build
|
||||
sudo ./z1_ctrl
|
||||
```
|
||||
|
||||
- Sencond, build the z1_sdk, and then open another terminal to run example.
|
||||
|
||||
```
|
||||
cd /<path to>/z1_sdk && mkdir build && cd build
|
||||
cmake ..
|
||||
make -j4
|
||||
./example_state_send
|
||||
```
|
||||
|
||||
### Low level control
|
||||
|
||||
```
|
||||
sudo ./z1_ctrl # Running in a terminal
|
||||
./example_lowCmd_send # Running in another terminal
|
||||
```
|
||||
|
||||
### Keyboard control
|
||||
|
||||
```
|
||||
sudo ./z1_ctrl # Running in a terminal
|
||||
./example_keyboard_send # Running in another terminal
|
||||
```
|
||||
|
|
@ -0,0 +1,53 @@
|
|||
/*****************************************************************
|
||||
Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved.
|
||||
*****************************************************************/
|
||||
#include "unitree_arm_sdk/unitree_arm_sdk.h"
|
||||
|
||||
bool running = true;
|
||||
|
||||
// over watch the ctrl+c command
|
||||
void ShutDown(int sig){
|
||||
std::cout << "[STATE] stop the controller" << std::endl;
|
||||
running = false;
|
||||
}
|
||||
|
||||
int main(){
|
||||
EmptyAction emptyAction((int)ArmFSMState::INVALID);
|
||||
std::vector<KeyAction*> events;
|
||||
CmdPanel *cmdPanel;
|
||||
events.push_back(new StateAction("`", (int)ArmFSMState::BACKTOSTART));
|
||||
events.push_back(new StateAction("1", (int)ArmFSMState::PASSIVE));
|
||||
events.push_back(new StateAction("2", (int)ArmFSMState::JOINTCTRL));
|
||||
events.push_back(new StateAction("3", (int)ArmFSMState::CARTESIAN));
|
||||
events.push_back(new StateAction("4", (int)ArmFSMState::MOVEJ));
|
||||
events.push_back(new StateAction("5", (int)ArmFSMState::MOVEL));
|
||||
events.push_back(new StateAction("6", (int)ArmFSMState::MOVEC));
|
||||
events.push_back(new StateAction("7", (int)ArmFSMState::TEACH));
|
||||
events.push_back(new StateAction("8", (int)ArmFSMState::TEACHREPEAT));
|
||||
events.push_back(new StateAction("9", (int)ArmFSMState::SAVESTATE));
|
||||
events.push_back(new StateAction("0", (int)ArmFSMState::TOSTATE));
|
||||
events.push_back(new StateAction("-", (int)ArmFSMState::TRAJECTORY));
|
||||
events.push_back(new StateAction("=", (int)ArmFSMState::CALIBRATION));
|
||||
events.push_back(new StateAction("]", (int)ArmFSMState::NEXT));
|
||||
events.push_back(new StateAction("/", (int)ArmFSMState::LOWCMD));
|
||||
|
||||
events.push_back(new ValueAction("q", "a", 1.0));
|
||||
events.push_back(new ValueAction("w", "s", 1.0));
|
||||
events.push_back(new ValueAction("e", "d", 1.0));
|
||||
events.push_back(new ValueAction("r", "f", 1.0));
|
||||
events.push_back(new ValueAction("t", "g", 1.0));
|
||||
events.push_back(new ValueAction("y", "h", 1.0));
|
||||
events.push_back(new ValueAction("down", "up", 1.0));
|
||||
|
||||
|
||||
cmdPanel = new UnitreeKeyboardUDPRecv(events, emptyAction);
|
||||
while(true){
|
||||
std::cout << "state:" << (int)cmdPanel->getState() << std::endl;
|
||||
std::cout << "value:" << cmdPanel->getValues().at(0) << std::endl;
|
||||
usleep(1000000);
|
||||
}
|
||||
|
||||
delete cmdPanel;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
/*****************************************************************
|
||||
Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved.
|
||||
*****************************************************************/
|
||||
#include "unitree_arm_sdk/unitree_arm_sdk.h"
|
||||
|
||||
bool running = true;
|
||||
|
||||
// over watch the ctrl+c command
|
||||
void ShutDown(int sig){
|
||||
std::cout << "[STATE] stop the controller" << std::endl;
|
||||
running = false;
|
||||
}
|
||||
|
||||
int main(){
|
||||
EmptyAction emptyAction((int)ArmFSMState::INVALID);
|
||||
std::vector<KeyAction*> events;
|
||||
UnitreeKeyboardUDPSend *cmdPanel;
|
||||
|
||||
events.push_back(new StateAction("`", (int)ArmFSMState::BACKTOSTART));
|
||||
events.push_back(new StateAction("1", (int)ArmFSMState::PASSIVE));
|
||||
events.push_back(new StateAction("2", (int)ArmFSMState::JOINTCTRL));
|
||||
events.push_back(new StateAction("3", (int)ArmFSMState::CARTESIAN));
|
||||
events.push_back(new StateAction("4", (int)ArmFSMState::MOVEJ));
|
||||
events.push_back(new StateAction("5", (int)ArmFSMState::MOVEL));
|
||||
events.push_back(new StateAction("6", (int)ArmFSMState::MOVEC));
|
||||
events.push_back(new StateAction("7", (int)ArmFSMState::TEACH));
|
||||
events.push_back(new StateAction("8", (int)ArmFSMState::TEACHREPEAT));
|
||||
events.push_back(new StateAction("9", (int)ArmFSMState::SAVESTATE));
|
||||
events.push_back(new StateAction("0", (int)ArmFSMState::TOSTATE));
|
||||
events.push_back(new StateAction("-", (int)ArmFSMState::TRAJECTORY));
|
||||
events.push_back(new StateAction("=", (int)ArmFSMState::CALIBRATION));
|
||||
events.push_back(new StateAction("]", (int)ArmFSMState::NEXT));
|
||||
events.push_back(new StateAction("/", (int)ArmFSMState::LOWCMD));
|
||||
|
||||
events.push_back(new ValueAction("q", "a", 1.0));
|
||||
events.push_back(new ValueAction("w", "s", 1.0));
|
||||
events.push_back(new ValueAction("e", "d", 1.0));
|
||||
events.push_back(new ValueAction("r", "f", 1.0));
|
||||
events.push_back(new ValueAction("t", "g", 1.0));
|
||||
events.push_back(new ValueAction("y", "h", 1.0));
|
||||
events.push_back(new ValueAction("down", "up", 1.0));
|
||||
|
||||
cmdPanel = new UnitreeKeyboardUDPSend(events, emptyAction);
|
||||
|
||||
while(running){
|
||||
usleep(1000000);
|
||||
}
|
||||
|
||||
delete cmdPanel;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,187 @@
|
|||
/*****************************************************************
|
||||
Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved.
|
||||
*****************************************************************/
|
||||
#include <math.h>
|
||||
#include "unitree_arm_sdk/unitree_arm_sdk.h"
|
||||
|
||||
struct ConfigPD{
|
||||
float K_P[7];
|
||||
float K_W[7];
|
||||
|
||||
virtual void setContrlGain()=0;
|
||||
};
|
||||
|
||||
struct SimulationPD : ConfigPD{
|
||||
|
||||
void setContrlGain(){
|
||||
K_P[0] = 80;
|
||||
K_P[1] = 200;
|
||||
K_P[2] = 200;
|
||||
K_P[3] = 15;
|
||||
K_P[4] = 15;
|
||||
K_P[5] = 15;
|
||||
K_P[6] = 15;
|
||||
|
||||
K_W[0] = 5;
|
||||
K_W[1] = 10;
|
||||
K_W[2] = 10;
|
||||
K_W[3] = 2;
|
||||
K_W[4] = 2;
|
||||
K_W[5] = 2;
|
||||
K_W[6] = 5;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct RealRobotPD : ConfigPD{
|
||||
|
||||
void setContrlGain(){
|
||||
K_P[0] = 20;
|
||||
K_P[1] = 40;
|
||||
K_P[2] = 20;
|
||||
K_P[3] = 160;
|
||||
K_P[4] = 160;
|
||||
K_P[5] = 160;
|
||||
K_P[6] = 20;
|
||||
|
||||
K_W[0] = 6000;
|
||||
K_W[1] = 6000;
|
||||
K_W[2] = 6000;
|
||||
K_W[3] = 6000;
|
||||
K_W[4] = 6000;
|
||||
K_W[5] = 6000;
|
||||
K_W[6] = 3000;
|
||||
}
|
||||
};
|
||||
|
||||
void printUsage() {
|
||||
printf(
|
||||
"Usage: controller [sim-or-robot]\n"
|
||||
"\t sim-or-robot: s for sim, r for robot\n");
|
||||
}
|
||||
|
||||
int main(int argc, char *argv []){
|
||||
// input number
|
||||
if (argc != 2) {
|
||||
printUsage();
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
|
||||
// PD config
|
||||
ConfigPD *configPD;
|
||||
if (argv[1][0] == 's') {
|
||||
configPD = new SimulationPD();
|
||||
configPD->setContrlGain();
|
||||
} else if (argv[1][0] == 'r') {
|
||||
configPD = new RealRobotPD();
|
||||
configPD->setContrlGain();
|
||||
} else {
|
||||
printUsage();
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
|
||||
// UDP object
|
||||
UDPPort udp("127.0.0.1", 8071, 8072, sizeof(RecvState), BlockYN::NO, 500000);
|
||||
|
||||
// Send object
|
||||
SendCmd sendCmd;
|
||||
RecvState recvState;
|
||||
sendCmd = {0};
|
||||
recvState = {0};
|
||||
sendCmd.head[0] = 0xFE;
|
||||
sendCmd.head[1] = 0xFF;
|
||||
|
||||
float deltaTime = 0.004; // s
|
||||
|
||||
// Only Passive state can change to LowCmd state
|
||||
while (recvState.state != ArmFSMState::PASSIVE){
|
||||
sendCmd.state = ArmFSMState::PASSIVE;
|
||||
udp.send((uint8_t*)&sendCmd, sizeof(SendCmd));
|
||||
udp.recv((uint8_t*)&recvState, sizeof(RecvState));
|
||||
usleep(deltaTime*1000000);
|
||||
}
|
||||
|
||||
// LowCmd state
|
||||
sendCmd.state = ArmFSMState::LOWCMD;
|
||||
for(uint8_t i(0); i<7; ++i){
|
||||
sendCmd.valueUnion.jointCmd[i].Pos = 0;
|
||||
sendCmd.valueUnion.jointCmd[i].W = 0;
|
||||
sendCmd.valueUnion.jointCmd[i].T = 0;
|
||||
sendCmd.valueUnion.jointCmd[i].K_P = configPD->K_P[i];
|
||||
sendCmd.valueUnion.jointCmd[i].K_W = configPD->K_W[i];
|
||||
}
|
||||
sendCmd.valueUnion.jointCmd[3].Pos = -M_PI*4.26/180;
|
||||
sendCmd.valueUnion.jointCmd[3].W = 0;
|
||||
sendCmd.valueUnion.jointCmd[3].T = 0;
|
||||
sendCmd.valueUnion.jointCmd[3].K_P = configPD->K_P[3];
|
||||
sendCmd.valueUnion.jointCmd[3].K_W = configPD->K_W[3];
|
||||
|
||||
int count = 0;
|
||||
float velocity = 1; // rad/s
|
||||
|
||||
for(int i(0); i<1000; ++i){
|
||||
if(i<500){
|
||||
// // torque
|
||||
// sendCmd.valueUnion.jointCmd[0].T = 2.5;
|
||||
// sendCmd.valueUnion.jointCmd[0].K_P = 0;
|
||||
// sendCmd.valueUnion.jointCmd[0].K_W = 0;
|
||||
|
||||
// // postion
|
||||
// sendCmd.valueUnion.jointCmd[0].Pos += deltaTime*velocity;
|
||||
// sendCmd.valueUnion.jointCmd[0].K_P = configPD->K_P[0];
|
||||
// sendCmd.valueUnion.jointCmd[0].K_W = configPD->K_W[0];
|
||||
|
||||
// // velocity
|
||||
// sendCmd.valueUnion.jointCmd[0].W = velocity;
|
||||
// sendCmd.valueUnion.jointCmd[0].K_P = 0;
|
||||
// sendCmd.valueUnion.jointCmd[0].K_W = configPD->K_W[0];
|
||||
|
||||
// Grepper
|
||||
sendCmd.valueUnion.jointCmd[6].Pos -= deltaTime*velocity;
|
||||
sendCmd.valueUnion.jointCmd[6].W = -velocity;
|
||||
sendCmd.valueUnion.jointCmd[6].K_P = configPD->K_P[6];
|
||||
sendCmd.valueUnion.jointCmd[6].K_W = configPD->K_W[6];
|
||||
|
||||
} else{
|
||||
// // torque
|
||||
// sendCmd.valueUnion.jointCmd[0].T = -2.5;
|
||||
// sendCmd.valueUnion.jointCmd[0].K_P = 0;
|
||||
// sendCmd.valueUnion.jointCmd[0].K_W = 0;
|
||||
|
||||
// // postion
|
||||
// sendCmd.valueUnion.jointCmd[0].Pos -= deltaTime*velocity;
|
||||
// sendCmd.valueUnion.jointCmd[0].K_P = configPD->K_P[0];
|
||||
// sendCmd.valueUnion.jointCmd[0].K_W = configPD->K_W[0];
|
||||
|
||||
// // velocity
|
||||
// sendCmd.valueUnion.jointCmd[0].W = -velocity;
|
||||
// sendCmd.valueUnion.jointCmd[0].K_P = 0;
|
||||
// sendCmd.valueUnion.jointCmd[0].K_W = configPD->K_W[0];
|
||||
|
||||
// Grepper
|
||||
sendCmd.valueUnion.jointCmd[6].Pos += deltaTime*velocity;
|
||||
sendCmd.valueUnion.jointCmd[6].W = velocity;
|
||||
sendCmd.valueUnion.jointCmd[6].K_P = configPD->K_P[6];
|
||||
sendCmd.valueUnion.jointCmd[6].K_W = configPD->K_W[6];
|
||||
}
|
||||
|
||||
udp.send((uint8_t*)&sendCmd, sizeof(SendCmd));
|
||||
udp.recv((uint8_t*)&recvState, sizeof(RecvState));
|
||||
|
||||
// printf("Pos:%f \n", recvState.jointState[0].Pos);
|
||||
// printf("roll:%f \n", recvState.cartesianState.roll);
|
||||
// printf("pitch:%f \n", recvState.cartesianState.pitch);
|
||||
// printf("yaw:%f \n", recvState.cartesianState.yaw);
|
||||
usleep(deltaTime*1000000);
|
||||
}
|
||||
|
||||
while (recvState.state != ArmFSMState::BACKTOSTART){
|
||||
sendCmd.state = ArmFSMState::BACKTOSTART;
|
||||
udp.send((uint8_t*)&sendCmd, sizeof(SendCmd));
|
||||
udp.recv((uint8_t*)&recvState, sizeof(RecvState));
|
||||
usleep(deltaTime*1000000);
|
||||
}
|
||||
|
||||
delete configPD;
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
/*****************************************************************
|
||||
Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved.
|
||||
*****************************************************************/
|
||||
#include "unitree_arm_sdk/unitree_arm_sdk.h"
|
||||
|
||||
int main(){
|
||||
UDPPort udp("127.0.0.1", 8072, 8071, sizeof(SendCmd), BlockYN::NO, 500000);
|
||||
|
||||
SendCmd sendCmd;
|
||||
RecvState recvState;
|
||||
sendCmd = {0};
|
||||
recvState = {0};
|
||||
|
||||
recvState.head[0] = 0xFE;
|
||||
recvState.head[1] = 0xFF;
|
||||
|
||||
bool turn = true;
|
||||
while(true){
|
||||
|
||||
udp.send((uint8_t*)&recvState, sizeof(RecvState));
|
||||
udp.recv((uint8_t*)&sendCmd, sizeof(SendCmd));
|
||||
|
||||
printf("%d \n", (int)sendCmd.state);
|
||||
usleep(2000);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,219 @@
|
|||
/*****************************************************************
|
||||
Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved.
|
||||
*****************************************************************/
|
||||
#include "unitree_arm_sdk/unitree_arm_sdk.h"
|
||||
|
||||
|
||||
SendCmd sendCmd;
|
||||
RecvState recvState;
|
||||
UDPPort udp("127.0.0.1", 8071, 8072, sizeof(RecvState), BlockYN::NO, 500000);
|
||||
|
||||
void UDPCommunication(){
|
||||
udp.send((uint8_t*)&sendCmd, sizeof(SendCmd));
|
||||
udp.recv((uint8_t*)&recvState, sizeof(RecvState));
|
||||
}
|
||||
|
||||
int main(){
|
||||
sendCmd = {0};
|
||||
recvState = {0};
|
||||
sendCmd.head[0] = 0xFE;
|
||||
sendCmd.head[1] = 0xFF;
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
|
||||
float deltaTime = 4000; // us
|
||||
|
||||
LoopFunc *udpCommunication;
|
||||
udpCommunication = new LoopFunc("udp", 0.002, boost::bind(&UDPCommunication));
|
||||
udpCommunication->start();
|
||||
|
||||
std::cout << "[PASSIVE]" << std::endl;
|
||||
sendCmd.state = ArmFSMState::PASSIVE;
|
||||
while (recvState.state != ArmFSMState::PASSIVE){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
|
||||
std::cout << "[BACKTOSTART]" << std::endl;
|
||||
sendCmd.state = ArmFSMState::BACKTOSTART;
|
||||
while (recvState.state != ArmFSMState::BACKTOSTART){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
while (recvState.state != ArmFSMState::JOINTCTRL){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
|
||||
std::cout << "[CARTESIAN]" << std::endl;
|
||||
sendCmd.state = ArmFSMState::CARTESIAN;
|
||||
while (recvState.state != ArmFSMState::CARTESIAN){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
|
||||
for(int i(0); i<2000; ++i){
|
||||
if(i<500){
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
sendCmd.value = ArmFSMValue::E;
|
||||
}
|
||||
else if(i<1000){
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
sendCmd.value = ArmFSMValue::Q;
|
||||
}
|
||||
else if(i<1500){
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
sendCmd.value = ArmFSMValue::D;
|
||||
}
|
||||
else{
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
sendCmd.value = ArmFSMValue::Q;
|
||||
}
|
||||
|
||||
// printf("------state:%d-------\n", (int)recvState.state);
|
||||
// printf("Pos:%f \n", recvState.jointState[0].Pos);
|
||||
// printf("Pos:%f \n", recvState.jointState[1].Pos);
|
||||
// printf("Pos:%f \n", recvState.jointState[2].Pos);
|
||||
// printf("roll:%f \n", recvState.cartesianState.roll);
|
||||
// printf("pitch:%f \n", recvState.cartesianState.pitch);
|
||||
// printf("yaw:%f \n", recvState.cartesianState.yaw);
|
||||
usleep(deltaTime);
|
||||
}
|
||||
|
||||
std::cout << "[JOINTCTRL]" << std::endl;
|
||||
sendCmd.state = ArmFSMState::JOINTCTRL;
|
||||
while (recvState.state != ArmFSMState::JOINTCTRL){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
|
||||
for(int i(0); i<1800; ++i){
|
||||
if(i<300){
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
sendCmd.value = ArmFSMValue::Q;
|
||||
}
|
||||
else if(i<600){
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
sendCmd.value = ArmFSMValue::W;
|
||||
}
|
||||
else if(i<900){
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
sendCmd.value = ArmFSMValue::D;
|
||||
}
|
||||
else if(i<1200){
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
sendCmd.value = ArmFSMValue::R;
|
||||
}
|
||||
else if(i<1500){
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
sendCmd.value = ArmFSMValue::T;
|
||||
}
|
||||
else{
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
sendCmd.value = ArmFSMValue::Y;
|
||||
}
|
||||
|
||||
// printf("------state:%d-------\n", (int)recvState.state);
|
||||
// printf("Pos:%f \n", recvState.jointState[0].Pos);
|
||||
// printf("Pos:%f \n", recvState.jointState[1].Pos);
|
||||
// printf("Pos:%f \n", recvState.jointState[2].Pos);
|
||||
// printf("roll:%f \n", recvState.cartesianState.roll);
|
||||
// printf("pitch:%f \n", recvState.cartesianState.pitch);
|
||||
// printf("yaw:%f \n", recvState.cartesianState.yaw);
|
||||
usleep(deltaTime);
|
||||
}
|
||||
|
||||
std::cout << "[SAVESTATE]" << std::endl;
|
||||
std::string test = "test";
|
||||
sendCmd.state = ArmFSMState::SAVESTATE;
|
||||
strcpy(sendCmd.valueUnion.saveState, test.c_str());
|
||||
usleep(deltaTime);
|
||||
|
||||
std::cout << "[BACKTOSTART]" << std::endl;
|
||||
sendCmd.state = ArmFSMState::BACKTOSTART;
|
||||
while(recvState.state != ArmFSMState::BACKTOSTART){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
while (recvState.state != ArmFSMState::JOINTCTRL){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
|
||||
|
||||
std::cout << "[TOSTATE]" << std::endl;
|
||||
sendCmd.state = ArmFSMState::TOSTATE;
|
||||
strcpy(sendCmd.valueUnion.toState, test.c_str());
|
||||
while (recvState.state != ArmFSMState::TOSTATE){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
while (recvState.state != ArmFSMState::JOINTCTRL){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
|
||||
std::cout << "[BACKTOSTART]" << std::endl;
|
||||
sendCmd.state = ArmFSMState::BACKTOSTART;
|
||||
while (recvState.state != ArmFSMState::BACKTOSTART){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
while (recvState.state != ArmFSMState::JOINTCTRL){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
|
||||
std::cout << "[MoveJ]" << std::endl;
|
||||
sendCmd.state = ArmFSMState::MOVEJ;
|
||||
sendCmd.valueUnion.moveJ.roll = 0;
|
||||
sendCmd.valueUnion.moveJ.pitch = 0;
|
||||
sendCmd.valueUnion.moveJ.yaw = 0;
|
||||
sendCmd.valueUnion.moveJ.x = 0.5;
|
||||
sendCmd.valueUnion.moveJ.y = -0.3;
|
||||
sendCmd.valueUnion.moveJ.z = 0.5;
|
||||
while (recvState.state != ArmFSMState::MOVEJ){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
while (recvState.state != ArmFSMState::JOINTCTRL){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
|
||||
std::cout << "[MoveL]" << std::endl;
|
||||
sendCmd.state = ArmFSMState::MOVEL;
|
||||
sendCmd.valueUnion.moveL.roll = 0;
|
||||
sendCmd.valueUnion.moveL.pitch = 0;
|
||||
sendCmd.valueUnion.moveL.yaw = 0;
|
||||
sendCmd.valueUnion.moveL.x = 0.5;
|
||||
sendCmd.valueUnion.moveL.y = 0.4;
|
||||
sendCmd.valueUnion.moveL.z = 0.3;
|
||||
while (recvState.state != ArmFSMState::MOVEL){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
while (recvState.state != ArmFSMState::JOINTCTRL){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
|
||||
std::cout << "[MoveC]" << std::endl;
|
||||
sendCmd.state = ArmFSMState::MOVEC;
|
||||
sendCmd.valueUnion.moveC[0].roll = 0;
|
||||
sendCmd.valueUnion.moveC[0].pitch = 0;
|
||||
sendCmd.valueUnion.moveC[0].yaw = 0;
|
||||
sendCmd.valueUnion.moveC[0].x = 0.5;
|
||||
sendCmd.valueUnion.moveC[0].y = 0;
|
||||
sendCmd.valueUnion.moveC[0].z = 0.5;
|
||||
sendCmd.valueUnion.moveC[1].roll = 0;
|
||||
sendCmd.valueUnion.moveC[1].pitch = 0;
|
||||
sendCmd.valueUnion.moveC[1].yaw = 0;
|
||||
sendCmd.valueUnion.moveC[1].x = 0.5;
|
||||
sendCmd.valueUnion.moveC[1].y = -0.4;
|
||||
sendCmd.valueUnion.moveC[1].z = 0.3;
|
||||
while (recvState.state != ArmFSMState::MOVEC){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
sendCmd.state = ArmFSMState::INVALID;
|
||||
while (recvState.state != ArmFSMState::JOINTCTRL){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
|
||||
std::cout << "[BACKTOSTART]" << std::endl;
|
||||
sendCmd.state = ArmFSMState::BACKTOSTART;
|
||||
while (recvState.state != ArmFSMState::BACKTOSTART){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,150 @@
|
|||
/*****************************************************************
|
||||
Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved.
|
||||
*****************************************************************/
|
||||
#ifndef _UNITREE_ARM_CMDPANEL_H_
|
||||
#define _UNITREE_ARM_CMDPANEL_H_
|
||||
|
||||
#include <vector>
|
||||
#include <deque>
|
||||
#include "unitree_arm_sdk/loop.h"
|
||||
#include "unitree_arm_sdk/common/arm_common.h"
|
||||
|
||||
|
||||
enum class KeyPress{
|
||||
RELEASE,
|
||||
PRESS,
|
||||
REPEAT
|
||||
};
|
||||
|
||||
|
||||
enum class ActionType{
|
||||
EMPTY,
|
||||
STATE,
|
||||
VALUE
|
||||
};
|
||||
|
||||
|
||||
struct KeyCmd{
|
||||
std::string c;
|
||||
KeyPress keyPress;
|
||||
};
|
||||
|
||||
|
||||
class KeyAction{
|
||||
public:
|
||||
KeyAction(ActionType type);
|
||||
virtual ~KeyAction(){};
|
||||
ActionType getType(){return _type;}
|
||||
protected:
|
||||
ActionType _type;
|
||||
};
|
||||
|
||||
|
||||
class StateAction : public KeyAction{
|
||||
public:
|
||||
StateAction(std::string c, int state, KeyPress press = KeyPress::PRESS);
|
||||
virtual ~StateAction(){};
|
||||
int getState(){return _state;};
|
||||
bool handleCmd(KeyCmd keyCmd, int &state);
|
||||
protected:
|
||||
int _state;
|
||||
KeyCmd _keyCmdSet;
|
||||
};
|
||||
|
||||
class EmptyAction : public StateAction{
|
||||
public:
|
||||
EmptyAction(int state);
|
||||
~EmptyAction(){};
|
||||
private:
|
||||
|
||||
};
|
||||
|
||||
|
||||
/*
|
||||
必须为长按, deltaValue为每秒改变量
|
||||
valueAction允许共用按键，例如空格停止
|
||||
但是正反转、停止键不可重复
|
||||
*/
|
||||
class ValueAction : public KeyAction{
|
||||
public:
|
||||
ValueAction(std::string cUp, std::string cDown, double deltaValue, double initValue = 0.0);
|
||||
ValueAction(std::string cUp, std::string cDown, std::string cGoZero, double deltaValue, double initValue = 0.0);
|
||||
ValueAction(std::string cUp, std::string cDown, double deltaValue, double limit1, double limit2, double initValue = 0.0);
|
||||
ValueAction(std::string cUp, std::string cDown, std::string cGoZero, double deltaValue, double limit1, double limit2, double initValue = 0.0);
|
||||
|
||||
~ValueAction(){};
|
||||
bool handleCmd(KeyCmd keyCmd);
|
||||
void setDt(double dt);
|
||||
double getValue();
|
||||
double getDValue();
|
||||
double setValue(double value){_value = value;}
|
||||
private:
|
||||
double _value;
|
||||
double _changeDirection;
|
||||
double _dV = 0.0;
|
||||
double _dt = 0.0;
|
||||
double _dVdt = 0.0;
|
||||
double _lim1, _lim2;
|
||||
bool _hasLim = false;
|
||||
bool _hasGoZero = false;
|
||||
|
||||
KeyCmd _upCmdSet;
|
||||
KeyCmd _downCmdSet;
|
||||
KeyCmd _goZeroCmdSet;
|
||||
|
||||
};
|
||||
|
||||
|
||||
class CmdPanel{
|
||||
public:
|
||||
CmdPanel(std::vector<KeyAction*> events,
|
||||
EmptyAction emptyAction, size_t channelNum = 1, double dt = 0.002);
|
||||
virtual ~CmdPanel();
|
||||
int getState(size_t channelID = 0);
|
||||
std::vector<double> getValues();
|
||||
std::vector<double> getDValues();
|
||||
void setValue(std::vector<double> values);
|
||||
void setValue(double value, size_t id);
|
||||
virtual std::string getString(std::string slogan);
|
||||
virtual std::vector<double> stringToArray(std::string slogan);
|
||||
virtual std::vector<std::vector<double> > stringToMatrix(std::string slogan);
|
||||
|
||||
virtual SendCmd getSendCmd();
|
||||
virtual void getRecvState(RecvState recvState);
|
||||
|
||||
protected:
|
||||
virtual void _read() = 0;
|
||||
void _start();
|
||||
void _run();
|
||||
void _updateState();
|
||||
void _pressKeyboard();
|
||||
void _releaseKeyboard();
|
||||
|
||||
static void* _runStatic(void* obj);
|
||||
static void* _readStatic(void* obj);
|
||||
|
||||
LoopFunc *_runThread;
|
||||
LoopFunc *_readThread;
|
||||
|
||||
std::vector<StateAction> _stateEvents;
|
||||
std::vector<ValueAction> _valueEvents;
|
||||
|
||||
EmptyAction _emptyAction;
|
||||
size_t _actionNum = 0;
|
||||
size_t _stateNum = 0;
|
||||
size_t _valueNum = 0;
|
||||
size_t _channelNum;
|
||||
std::vector<double> _values;
|
||||
std::vector<double> _dValues;
|
||||
int _state;
|
||||
std::vector<std::deque<int>> _stateQueue;
|
||||
std::vector<int> _outputState;
|
||||
std::vector<bool> _getState;
|
||||
double _dt;
|
||||
KeyCmd _keyCmd;
|
||||
std::string _cPast = "";
|
||||
|
||||
bool _running = true;
|
||||
};
|
||||
|
||||
#endif // _UNITREE_ARM_CMDPANEL_H_
|
||||
|
|
@ -0,0 +1,130 @@
|
|||
#ifndef _UNITREE_ARM_ARM_COMMON_H_
|
||||
#define _UNITREE_ARM_ARM_COMMON_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#pragma pack(1)
|
||||
|
||||
// 4 Byte
|
||||
enum class ArmFSMState{
|
||||
INVALID,
|
||||
PASSIVE,
|
||||
JOINTCTRL,
|
||||
CARTESIAN,
|
||||
MOVEJ,
|
||||
MOVEL,
|
||||
MOVEC,
|
||||
TRAJECTORY,
|
||||
TOSTATE,
|
||||
SAVESTATE,
|
||||
TEACH,
|
||||
TEACHREPEAT,
|
||||
CALIBRATION,
|
||||
DANCE00,
|
||||
DANCE01,
|
||||
DANCE02,
|
||||
DANCE03,
|
||||
DANCE04,
|
||||
DANCE05,
|
||||
DANCE06,
|
||||
DANCE07,
|
||||
DANCE08,
|
||||
DANCE09,
|
||||
BACKTOSTART,
|
||||
GRIPPER_OPEN,
|
||||
GRIPPER_CLOSE,
|
||||
NEXT,
|
||||
LOWCMD
|
||||
};
|
||||
|
||||
// 4 Byte
|
||||
enum class ArmFSMValue{
|
||||
INVALID,
|
||||
Q,A,
|
||||
W,S,
|
||||
E,D,
|
||||
R,F,
|
||||
T,G,
|
||||
Y,H,
|
||||
DOWN,
|
||||
UP
|
||||
};
|
||||
|
||||
// 20 Byte
|
||||
struct JointCmd{
|
||||
float T;
|
||||
float W;
|
||||
float Pos;
|
||||
float K_P;
|
||||
float K_W;
|
||||
};
|
||||
|
||||
// 16 Byte
|
||||
struct JointState{
|
||||
float T;
|
||||
float W;
|
||||
float Acc;
|
||||
float Pos;
|
||||
};
|
||||
|
||||
// 140 Byte
|
||||
union UDPSendCmd{
|
||||
uint8_t checkCmd;
|
||||
JointCmd jointCmd[7];
|
||||
};
|
||||
|
||||
// 16*7=112 Byte
|
||||
union UDPRecvState{
|
||||
uint8_t singleState;
|
||||
uint8_t selfCheck[10];
|
||||
JointState jointState[7];
|
||||
uint8_t errorCheck[16];
|
||||
};
|
||||
|
||||
// 24 Byte
|
||||
struct Posture{
|
||||
float roll;
|
||||
float pitch;
|
||||
float yaw;
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
};
|
||||
|
||||
// 48 Byte
|
||||
struct MoveC{
|
||||
Posture middlePosture;
|
||||
Posture endPosture;
|
||||
};
|
||||
|
||||
// 20*7=140 Byte
|
||||
union ValueUnion{
|
||||
Posture moveJ;
|
||||
Posture moveL;
|
||||
Posture moveC[2];
|
||||
char teach[10];
|
||||
char teachRepeat[10];
|
||||
char saveState[10];
|
||||
char toState[10];
|
||||
JointCmd jointCmd[7];
|
||||
};
|
||||
|
||||
// 2+4+4+140 = 150 Byte
|
||||
struct SendCmd{
|
||||
uint8_t head[2];
|
||||
ArmFSMState state;
|
||||
ArmFSMValue value;
|
||||
ValueUnion valueUnion;
|
||||
};
|
||||
|
||||
// 2+4+112+24 = 142 Byte
|
||||
struct RecvState{
|
||||
uint8_t head[2];
|
||||
ArmFSMState state;
|
||||
JointState jointState[7];
|
||||
Posture cartesianState;
|
||||
};
|
||||
|
||||
#pragma pack()
|
||||
|
||||
#endif // _UNITREE_ARM_ARM_MSG_H_
|
||||
|
|
@ -0,0 +1,214 @@
|
|||
#ifndef _UNITREE_ARM_ARM_MOTOR_COMMON_H_
|
||||
#define _UNITREE_ARM_ARM_MOTOR_COMMON_H_
|
||||
|
||||
#include <stdint.h>
|
||||
typedef int16_t q15_t;
|
||||
|
||||
#pragma pack(1)
|
||||
|
||||
// 发送用单个数据数据结构
|
||||
typedef union{
|
||||
int32_t L;
|
||||
uint8_t u8[4];
|
||||
uint16_t u16[2];
|
||||
uint32_t u32;
|
||||
float F;
|
||||
}COMData32;
|
||||
|
||||
typedef struct {
|
||||
// 定义 数据包头
|
||||
unsigned char start[2]; // 包头
|
||||
unsigned char motorID; // 电机ID 0,1,2,3 ... 0xBB 表示向所有电机广播（此时无返回）
|
||||
unsigned char reserved;
|
||||
}COMHead;
|
||||
|
||||
#pragma pack()
|
||||
|
||||
#pragma pack(1)
|
||||
|
||||
typedef struct {
|
||||
|
||||
uint8_t fan_d; // 关节上的散热风扇转速
|
||||
uint8_t Fmusic; // 电机发声频率 /64*1000 15.625f 频率分度
|
||||
uint8_t Hmusic; // 电机发声强度 推荐值4 声音强度 0.1 分度
|
||||
uint8_t reserved4;
|
||||
|
||||
uint8_t FRGB[4]; // 足端LED
|
||||
|
||||
}LowHzMotorCmd;
|
||||
|
||||
typedef struct { // 以 4个字节一组排列 ，不然编译器会凑整
|
||||
// 定义 数据
|
||||
uint8_t mode; // 关节模式选择
|
||||
uint8_t ModifyBit; // 电机控制参数修改位
|
||||
uint8_t ReadBit; // 电机控制参数发送位
|
||||
uint8_t reserved;
|
||||
|
||||
COMData32 Modify; // 电机参数修改 的数据
|
||||
//实际给FOC的指令力矩为：
|
||||
//K_P*delta_Pos + K_W*delta_W + T
|
||||
q15_t T; // 期望关节的输出力矩（电机本身的力矩）x256, 7 + 8 描述
|
||||
q15_t W; // 期望关节速度 （电机本身的速度） x128, 8 + 7描述
|
||||
int32_t Pos; // 期望关节位置 x 16384/6.2832, 14位编码器（主控0点修正，电机关节还是以编码器0点为准）
|
||||
|
||||
q15_t K_P; // 关节刚度系数 x2048 4+11 描述
|
||||
q15_t K_W; // 关节速度系数 x1024 5+10 描述
|
||||
|
||||
uint8_t LowHzMotorCmdIndex; // 电机低频率控制命令的索引, 0-7, 分别代表LowHzMotorCmd中的8个字节
|
||||
uint8_t LowHzMotorCmdByte; // 电机低频率控制命令的字节
|
||||
|
||||
COMData32 Res[1]; // 通讯 保留字节 用于实现别的一些通讯内容
|
||||
|
||||
}MasterComdV3; // 加上数据包的包头 和CRC 34字节
|
||||
|
||||
typedef struct {
|
||||
// 定义 电机控制命令数据包
|
||||
COMHead head;
|
||||
MasterComdV3 Mdata;
|
||||
COMData32 CRCdata;
|
||||
}MasterComdDataV3;//返回数据
|
||||
|
||||
// typedef struct {
|
||||
// // 定义 总得485 数据包
|
||||
|
||||
// MasterComdData M1;
|
||||
// MasterComdData M2;
|
||||
// MasterComdData M3;
|
||||
|
||||
// }DMA485TxDataV3;
|
||||
|
||||
#pragma pack()
|
||||
|
||||
#pragma pack(1)
|
||||
|
||||
typedef struct { // 以 4个字节一组排列 ，不然编译器会凑整
|
||||
// 定义 数据
|
||||
uint8_t mode; // 当前关节模式
|
||||
uint8_t ReadBit; // 电机控制参数修改 是否成功位
|
||||
int8_t Temp; // 电机当前平均温度
|
||||
uint8_t MError; // 电机错误 标识
|
||||
|
||||
COMData32 Read; // 读取的当前 电机 的控制数据
|
||||
int16_t T; // 当前实际电机输出力矩 7 + 8 描述
|
||||
|
||||
int16_t W; // 当前实际电机速度（高速） 8 + 7 描述
|
||||
float LW; // 当前实际电机速度（低速）
|
||||
|
||||
int16_t W2; // 当前实际关节速度（高速） 8 + 7 描述
|
||||
float LW2; // 当前实际关节速度（低速）
|
||||
|
||||
int16_t Acc; // 电机转子加速度 15+0 描述 惯量较小
|
||||
int16_t OutAcc; // 输出轴加速度 12+3 描述 惯量较大
|
||||
|
||||
int32_t Pos; // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准）
|
||||
int32_t Pos2; // 关节编码器位置(输出编码器)
|
||||
|
||||
int16_t gyro[3]; // 电机驱动板6轴传感器数据
|
||||
int16_t acc[3];
|
||||
|
||||
// 力传感器的数据
|
||||
int16_t Fgyro[3]; //
|
||||
int16_t Facc[3];
|
||||
int16_t Fmag[3];
|
||||
uint8_t Ftemp; // 8位表示的温度 7位（-28~100度） 1位0.5度分辨率
|
||||
|
||||
int16_t Force16; // 力传感器高16位数据
|
||||
int8_t Force8; // 力传感器低8位数据
|
||||
|
||||
uint8_t FError; // 足端传感器错误标识
|
||||
|
||||
int8_t Res[1]; // 通讯 保留字节
|
||||
|
||||
}ServoComdV3; // 加上数据包的包头 和CRC 78字节（4+70+4）
|
||||
|
||||
typedef struct {
|
||||
// 定义 电机控制命令数据包
|
||||
COMHead head;
|
||||
ServoComdV3 Mdata;
|
||||
|
||||
COMData32 CRCdata;
|
||||
|
||||
}ServoComdDataV3; //发送数据
|
||||
|
||||
// typedef struct {
|
||||
// // 定义 总的485 接受数据包
|
||||
|
||||
// ServoComdDataV3 M[3];
|
||||
// // uint8_t nullbyte1;
|
||||
|
||||
// }DMA485RxDataV3;
|
||||
|
||||
|
||||
#pragma pack()
|
||||
|
||||
// 00 00 00 00 00
|
||||
// 00 00 00 00 00
|
||||
// 00 00 00 00 00
|
||||
// 00 00 00
|
||||
// 数据包默认初始化
|
||||
// 主机发送的数据包
|
||||
/*
|
||||
Tx485Data[_FR][i].head.start[0] = 0xFE ; Tx485Data[_FR][i].head.start[1] = 0xEE; // 数据包头
|
||||
Tx485Data[_FR][i].Mdata.ModifyBit = 0xFF; Tx485Data[_FR][i].Mdata.mode = 0; // 默认不修改数据 和 电机的默认工作模式
|
||||
Tx485Data[_FR][i].head.motorID = i; 0 // 目标电机标号
|
||||
Tx485Data[_FR][i].Mdata.T = 0.0f; // 默认目标关节输出力矩 motor1.Extra_Torque = motorRxData[1].Mdata.T*0.390625f; // N.M 转化为 N.CM IQ8描述
|
||||
Tx485Data[_FR][i].Mdata.Pos = 0x7FE95C80; // 默认目标关节位置 不启用位置环 14位分辨率
|
||||
Tx485Data[_FR][i].Mdata.W = 16000.0f; // 默认目标关节速度 不启用速度环 1+8+7描述 motor1.Target_Speed = motorRxData[1].Mdata.W*0.0078125f; // 单位 rad/s IQ7描述
|
||||
Tx485Data[_FR][i].Mdata.K_P = (q15_t)(0.6f*(1<<11)); // 默认关节刚度系数 4+11 描述 motor1.K_Pos = ((float)motorRxData[1].Mdata.K_P)/(1<<11); // 描述刚度的通讯数据格式 4+11
|
||||
Tx485Data[_FR][i].Mdata.K_W = (q15_t)(1.0f*(1<<10)); // 默认关节速度系数 5+10 描述 motor1.K_Speed = ((float)motorRxData[1].Mdata.K_W)/(1<<10); // 描述阻尼的通讯数据格式 5+10
|
||||
*/
|
||||
|
||||
enum class MotorType{
|
||||
A1Go1, // 4.8M baudrate, K_W x1024
|
||||
B1 // 6.0M baudrate, K_W x512
|
||||
};
|
||||
|
||||
struct MOTOR_send{
|
||||
// 定义 发送格式化数据
|
||||
MasterComdDataV3 motor_send_data; //电机控制数据结构体
|
||||
MotorType motorType = MotorType::A1Go1;
|
||||
int hex_len = 34; //发送的16进制命令数组长度, 34
|
||||
// long long send_time; //发送该命令的时间, 微秒(us)
|
||||
// 待发送的各项数据
|
||||
unsigned short id; //电机ID，0xBB代表全部电机
|
||||
unsigned short mode; //0:空闲, 5:开环转动, 10:闭环FOC控制
|
||||
//实际给FOC的指令力矩为：
|
||||
//K_P*delta_Pos + K_W*delta_W + T
|
||||
float T; //期望关节的输出力矩（电机本身的力矩）（Nm）
|
||||
float W; //期望关节速度（电机本身的速度）(rad/s)
|
||||
float Pos; //期望关节位置（rad）
|
||||
float K_P; //关节刚度系数
|
||||
float K_W; //关节速度系数
|
||||
COMData32 Res; // 通讯 保留字节 用于实现别的一些通讯内容
|
||||
};
|
||||
|
||||
struct MOTOR_recv{
|
||||
// 定义 接收数据
|
||||
ServoComdDataV3 motor_recv_data; //电机接收数据结构体
|
||||
MotorType motorType = MotorType::A1Go1;
|
||||
int hex_len = 78; //接收的16进制命令数组长度, 78
|
||||
// long long resv_time; //接收该命令的时间, 微秒(us)
|
||||
bool correct = false; //接收数据是否完整（true完整，false不完整）
|
||||
//解读得出的电机数据
|
||||
unsigned char motor_id; //电机ID
|
||||
unsigned char mode; //0:空闲, 5:开环转动, 10:闭环FOC控制
|
||||
int Temp; //温度
|
||||
unsigned char MError; //错误码
|
||||
|
||||
float T; // 当前实际电机输出力矩
|
||||
float W; // 当前实际电机速度（高速）
|
||||
float LW; // 当前实际电机速度（低速）
|
||||
int Acc; // 电机转子加速度
|
||||
float Pos; // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准）
|
||||
|
||||
float gyro[3]; // 电机驱动板6轴传感器数据
|
||||
float acc[3];
|
||||
|
||||
int8_t Res;
|
||||
};
|
||||
|
||||
|
||||
extern void modify_data(MOTOR_send* motor_s);
|
||||
extern bool extract_data(MOTOR_recv* motor_r);
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,43 @@
|
|||
/*****************************************************************
|
||||
Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved.
|
||||
*****************************************************************/
|
||||
#ifndef _UNITREE_ARM_JOYSTICK_COMMON_H_
|
||||
#define _UNITREE_ARM_JOYSTICK_COMMON_H_
|
||||
#include <stdint.h>
|
||||
|
||||
// 16b
|
||||
typedef union {
|
||||
struct {
|
||||
uint8_t R1 :1;
|
||||
uint8_t L1 :1;
|
||||
uint8_t start :1;
|
||||
uint8_t select :1;
|
||||
uint8_t R2 :1;
|
||||
uint8_t L2 :1;
|
||||
uint8_t F1 :1;
|
||||
uint8_t F2 :1;
|
||||
uint8_t A :1;
|
||||
uint8_t B :1;
|
||||
uint8_t X :1;
|
||||
uint8_t Y :1;
|
||||
uint8_t up :1;
|
||||
uint8_t right :1;
|
||||
uint8_t down :1;
|
||||
uint8_t left :1;
|
||||
} components;
|
||||
uint16_t value;
|
||||
} xKeySwitchUnion;
|
||||
|
||||
// 40 Byte (now used 24B)
|
||||
typedef struct {
|
||||
uint8_t head[2];
|
||||
xKeySwitchUnion btn;
|
||||
float lx;
|
||||
float rx;
|
||||
float ry;
|
||||
float L2;
|
||||
float ly;
|
||||
uint8_t idle[16];
|
||||
} xRockerBtnDataStruct;
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,212 @@
|
|||
#ifndef _UNITREE_ARM_QUADRUPED_COMMON_H_
|
||||
#define _UNITREE_ARM_QUADRUPED_COMMON_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <array>
|
||||
|
||||
namespace UNITREE_LEGGED_SDK
|
||||
{
|
||||
|
||||
constexpr int HIGHLEVEL = 0xee;
|
||||
constexpr int LOWLEVEL = 0xff;
|
||||
constexpr int TRIGERLEVEL = 0xf0;
|
||||
constexpr double PosStopF = (2.146E+9f);
|
||||
constexpr double VelStopF = (16000.0f);
|
||||
extern const int HIGH_CMD_LENGTH; // sizeof(HighCmd)
|
||||
extern const int HIGH_STATE_LENGTH; // sizeof(HighState)
|
||||
extern const int LOW_CMD_LENGTH; // shorter than sizeof(LowCmd), bytes compressed LowCmd length
|
||||
extern const int LOW_STATE_LENGTH; // shorter than sizeof(LowState), bytes compressed LowState length
|
||||
|
||||
#pragma pack(1)
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t off; // off 0xA5
|
||||
std::array<uint8_t, 3> reserve;
|
||||
} BmsCmd;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t version_h;
|
||||
uint8_t version_l;
|
||||
uint8_t bms_status;
|
||||
uint8_t SOC; // SOC 0-100%
|
||||
int32_t current; // mA
|
||||
uint16_t cycle;
|
||||
std::array<int8_t, 8> BQ_NTC; // x1 degrees centigrade
|
||||
std::array<int8_t, 8> MCU_NTC; // x1 degrees centigrade
|
||||
std::array<uint16_t, 30> cell_vol; // cell voltage mV
|
||||
} BmsState;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
} Cartesian;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
std::array<float, 4> quaternion; // quaternion, normalized, (w,x,y,z)
|
||||
std::array<float, 3> gyroscope; // angular velocity （unit: rad/s) (raw data)
|
||||
std::array<float, 3> accelerometer; // m/(s2) (raw data)
|
||||
std::array<float, 3> rpy; // euler angle（unit: rad)
|
||||
int8_t temperature;
|
||||
} IMU; // when under accelerated motion, the attitude of the robot calculated by IMU will drift.
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t r;
|
||||
uint8_t g;
|
||||
uint8_t b;
|
||||
} LED; // foot led brightness: 0~255
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t mode; // motor working mode
|
||||
float q; // current angle (unit: radian)
|
||||
float dq; // current velocity (unit: radian/second)
|
||||
float ddq; // current acc (unit: radian/second*second)
|
||||
float tauEst; // current estimated output torque (unit: N.m)
|
||||
float q_raw; // current angle (unit: radian)
|
||||
float dq_raw; // current velocity (unit: radian/second)
|
||||
float ddq_raw;
|
||||
int8_t temperature; // current temperature (temperature conduction is slow that leads to lag)
|
||||
std::array<uint32_t, 2> reserve;
|
||||
} MotorState; // motor feedback
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t mode; // desired working mode
|
||||
float q; // desired angle (unit: radian)
|
||||
float dq; // desired velocity (unit: radian/second)
|
||||
float tau; // desired output torque (unit: N.m)
|
||||
float Kp; // desired position stiffness (unit: N.m/rad )
|
||||
float Kd; // desired velocity stiffness (unit: N.m/(rad/s) )
|
||||
std::array<uint32_t, 3> reserve;
|
||||
} MotorCmd; // motor control
|
||||
|
||||
typedef struct
|
||||
{
|
||||
std::array<uint8_t, 2> head;
|
||||
uint8_t levelFlag;
|
||||
uint8_t frameReserve;
|
||||
|
||||
std::array<uint32_t, 2> SN;
|
||||
std::array<uint32_t, 2> version;
|
||||
uint16_t bandWidth;
|
||||
IMU imu;
|
||||
std::array<MotorState, 20> motorState;
|
||||
BmsState bms;
|
||||
std::array<int16_t, 4> footForce; // force sensors
|
||||
std::array<int16_t, 4> footForceEst; // force sensors
|
||||
uint32_t tick; // reference real-time from motion controller (unit: us)
|
||||
std::array<uint8_t, 40> wirelessRemote; // wireless commands
|
||||
uint32_t reserve;
|
||||
|
||||
uint32_t crc;
|
||||
} LowState; // low level feedback
|
||||
|
||||
typedef struct
|
||||
{
|
||||
std::array<uint8_t, 2> head;
|
||||
uint8_t levelFlag;
|
||||
uint8_t frameReserve;
|
||||
|
||||
std::array<uint32_t, 2> SN;
|
||||
std::array<uint32_t, 2> version;
|
||||
uint16_t bandWidth;
|
||||
std::array<MotorCmd, 20> motorCmd;
|
||||
BmsCmd bms;
|
||||
std::array<uint8_t, 40> wirelessRemote;
|
||||
uint32_t reserve;
|
||||
|
||||
uint32_t crc;
|
||||
} LowCmd; // low level control
|
||||
|
||||
typedef struct
|
||||
{
|
||||
std::array<uint8_t, 2> head;
|
||||
uint8_t levelFlag;
|
||||
uint8_t frameReserve;
|
||||
|
||||
std::array<uint32_t, 2> SN;
|
||||
std::array<uint32_t, 2> version;
|
||||
uint16_t bandWidth;
|
||||
IMU imu;
|
||||
std::array<MotorState, 20> motorState;
|
||||
BmsState bms;
|
||||
std::array<int16_t, 4> footForce;
|
||||
std::array<int16_t, 4> footForceEst;
|
||||
uint8_t mode;
|
||||
float progress;
|
||||
uint8_t gaitType; // 0.idle 1.trot 2.trot running 3.climb stair 4.trot obstacle
|
||||
float footRaiseHeight; // (unit: m, default: 0.08m), foot up height while walking
|
||||
std::array<float, 3> position; // (unit: m), from own odometry in inertial frame, usually drift
|
||||
float bodyHeight; // (unit: m, default: 0.28m),
|
||||
std::array<float, 3> velocity; // (unit: m/s), forwardSpeed, sideSpeed, rotateSpeed in body frame
|
||||
float yawSpeed; // (unit: rad/s), rotateSpeed in body frame
|
||||
std::array<float, 4> rangeObstacle;
|
||||
std::array<Cartesian, 4> footPosition2Body; // foot position relative to body
|
||||
std::array<Cartesian, 4> footSpeed2Body; // foot speed relative to body
|
||||
std::array<uint8_t, 40> wirelessRemote;
|
||||
uint32_t reserve;
|
||||
|
||||
uint32_t crc;
|
||||
} HighState; // high level feedback
|
||||
|
||||
typedef struct
|
||||
{
|
||||
std::array<uint8_t, 2> head;
|
||||
uint8_t levelFlag;
|
||||
uint8_t frameReserve;
|
||||
|
||||
std::array<uint32_t, 2> SN;
|
||||
std::array<uint32_t, 2> version;
|
||||
uint16_t bandWidth;
|
||||
uint8_t mode; // 0. idle, default stand 1. force stand (controlled by dBodyHeight + ypr)
|
||||
// 2. target velocity walking (controlled by velocity + yawSpeed)
|
||||
// 3. target position walking (controlled by position + ypr[0])
|
||||
// 4. path mode walking (reserve for future release)
|
||||
// 5. position stand down.
|
||||
// 6. position stand up
|
||||
// 7. damping mode
|
||||
// 8. recovery stand
|
||||
// 9. backflip
|
||||
// 10. jumpYaw
|
||||
// 11. straightHand
|
||||
// 12. dance1
|
||||
// 13. dance2
|
||||
|
||||
uint8_t gaitType; // 0.idle 1.trot 2.trot running 3.climb stair 4.trot obstacle
|
||||
uint8_t speedLevel; // 0. default low speed. 1. medium speed 2. high speed. during walking, only respond MODE 3
|
||||
float footRaiseHeight; // (unit: m, default: 0.08m), foot up height while walking, delta value
|
||||
float bodyHeight; // (unit: m, default: 0.28m), delta value
|
||||
std::array<float, 2> position; // (unit: m), desired position in inertial frame
|
||||
std::array<float, 3> euler; // (unit: rad), roll pitch yaw in stand mode
|
||||
std::array<float, 2> velocity; // (unit: m/s), forwardSpeed, sideSpeed in body frame
|
||||
float yawSpeed; // (unit: rad/s), rotateSpeed in body frame
|
||||
BmsCmd bms;
|
||||
std::array<LED, 4> led;
|
||||
std::array<uint8_t, 40> wirelessRemote;
|
||||
uint32_t reserve;
|
||||
|
||||
uint32_t crc;
|
||||
} HighCmd; // high level control
|
||||
|
||||
#pragma pack()
|
||||
|
||||
typedef struct
|
||||
{
|
||||
unsigned long long TotalCount; // total loop count
|
||||
unsigned long long SendCount; // total send count
|
||||
unsigned long long RecvCount; // total receive count
|
||||
unsigned long long SendError; // total send error
|
||||
unsigned long long FlagError; // total flag error
|
||||
unsigned long long RecvCRCError; // total reveive CRC error
|
||||
unsigned long long RecvLoseError; // total lose package count
|
||||
} UDPState; // UDP communication state
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
#ifndef _UNITREE_ARM_JOYSTICK_H_
|
||||
#define _UNITREE_ARM_JOYSTICK_H_
|
||||
|
||||
#include <vector>
|
||||
#include "unitree_arm_sdk/udp.h"
|
||||
#include "unitree_arm_sdk/cmdPanel.h"
|
||||
#include "unitree_arm_sdk/common/quadruped_common.h"
|
||||
#include "unitree_arm_sdk/common/joystick_common.h"
|
||||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
|
||||
class UnitreeJoystick : public CmdPanel{
|
||||
public:
|
||||
UnitreeJoystick(std::vector<KeyAction*> events,
|
||||
EmptyAction emptyAction, size_t channelNum = 1,
|
||||
double dt = 0.002);
|
||||
~UnitreeJoystick();
|
||||
private:
|
||||
void _read();
|
||||
void _extractCmd();
|
||||
|
||||
xRockerBtnDataStruct _keyData;
|
||||
UDPPort *_udp;
|
||||
HighCmd _udpCmd;
|
||||
HighState _udpState;
|
||||
};
|
||||
|
||||
#endif // _UNITREE_ARM_JOYSTICK_H_
|
||||
|
|
@ -0,0 +1,92 @@
|
|||
#ifndef _UNITREE_ARM_KEYBOARD_H_
|
||||
#define _UNITREE_ARM_KEYBOARD_H_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <deque>
|
||||
|
||||
#include "unitree_arm_sdk/udp.h"
|
||||
#include "unitree_arm_sdk/common/arm_common.h"
|
||||
#include "unitree_arm_sdk/cmdPanel.h"
|
||||
|
||||
class Keyboard : public CmdPanel{
|
||||
public:
|
||||
Keyboard(std::vector<KeyAction*> events,
|
||||
EmptyAction emptyAction, size_t channelNum = 1, double dt = 0.002);
|
||||
~Keyboard();
|
||||
std::string getString(std::string slogan);
|
||||
std::vector<double> stringToArray(std::string slogan);
|
||||
std::vector<std::vector<double> > stringToMatrix(std::string slogan);
|
||||
private:
|
||||
void _read();
|
||||
void _pauseKey();
|
||||
void _startKey();
|
||||
void _extractCmd();
|
||||
|
||||
fd_set _set;
|
||||
char _c = '\0';
|
||||
|
||||
termios _oldSettings;
|
||||
termios _newSettings;
|
||||
timeval _tv;
|
||||
};
|
||||
|
||||
|
||||
class UnitreeKeyboardUDPSend : public CmdPanel{
|
||||
public:
|
||||
UnitreeKeyboardUDPSend(std::vector<KeyAction*> events,
|
||||
EmptyAction emptyAction, size_t channelNum = 1,
|
||||
double dt = 0.002);
|
||||
~UnitreeKeyboardUDPSend();
|
||||
std::string getString(std::string slogan);
|
||||
std::vector<double> stringToArray(std::string slogan);
|
||||
std::vector<std::vector<double> > stringToMatrix(std::string slogan);
|
||||
|
||||
private:
|
||||
// keyboard communication
|
||||
void _read();
|
||||
void _pauseKey();
|
||||
void _startKey();
|
||||
void _extractCmd();
|
||||
|
||||
fd_set _set;
|
||||
char _c = '\0';
|
||||
|
||||
termios _oldSettings;
|
||||
termios _newSettings;
|
||||
timeval _tv;
|
||||
|
||||
// udp communication
|
||||
void _communicationUDP();
|
||||
void _extractCmdKeyboard();
|
||||
|
||||
UDPPort *_udp;
|
||||
SendCmd _sendCmd;
|
||||
RecvState _recvState;
|
||||
std::vector<std::vector<double>> posture;
|
||||
std::string slogan;
|
||||
};
|
||||
|
||||
|
||||
class UnitreeKeyboardUDPRecv : public CmdPanel{
|
||||
public:
|
||||
UnitreeKeyboardUDPRecv(std::vector<KeyAction*> events,
|
||||
EmptyAction emptyAction, size_t channelNum = 1,
|
||||
double dt = 0.002);
|
||||
~UnitreeKeyboardUDPRecv();
|
||||
|
||||
SendCmd getSendCmd(){return _sendCmd;};
|
||||
void getRecvState(RecvState recvState){
|
||||
_recvState = recvState;
|
||||
};
|
||||
|
||||
private:
|
||||
void _read();
|
||||
void _extractCmd();
|
||||
|
||||
UDPPort *_udp;
|
||||
SendCmd _sendCmd;
|
||||
RecvState _recvState;
|
||||
};
|
||||
|
||||
#endif // _UNITREE_ARM_KEYBOARD_H_
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
#ifndef _UNITREE_ARM_LOOP_H_
|
||||
#define _UNITREE_ARM_LOOP_H_
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <thread>
|
||||
#include <pthread.h>
|
||||
#include <vector>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
#include "unitree_arm_sdk/timer.h"
|
||||
|
||||
// constexpr int THREAD_PRIORITY = 99; // real-time priority
|
||||
|
||||
typedef boost::function<void ()> Callback;
|
||||
|
||||
class Loop {
|
||||
public:
|
||||
Loop(std::string name, float period, int bindCPU = -1);
|
||||
~Loop();
|
||||
void start();
|
||||
void shutdown();
|
||||
virtual void functionCB() = 0;
|
||||
|
||||
private:
|
||||
void entryFunc();
|
||||
|
||||
std::string _name;
|
||||
float _period;
|
||||
int _bindCPU;
|
||||
bool _bind_cpu_flag = false;
|
||||
bool _isrunning = false;
|
||||
std::thread _thread;
|
||||
|
||||
size_t _runTimes = 0;
|
||||
size_t _timeOutTimes = 0;
|
||||
|
||||
AbsoluteTimer *_timer;
|
||||
};
|
||||
|
||||
class LoopFunc : public Loop {
|
||||
public:
|
||||
LoopFunc(std::string name, float period, const Callback& _cb)
|
||||
: Loop(name, period), _fp(_cb){}
|
||||
LoopFunc(std::string name, float period, int bindCPU, const Callback& _cb)
|
||||
: Loop(name, period, bindCPU), _fp(_cb){}
|
||||
void functionCB() { (_fp)(); }
|
||||
private:
|
||||
boost::function<void ()> _fp;
|
||||
};
|
||||
|
||||
#endif // _UNITREE_ARM_LOOP_H_
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
#ifndef _UNITREE_ARM_TIMER_H_
|
||||
#define _UNITREE_ARM_TIMER_H_
|
||||
|
||||
#include <iostream>
|
||||
#include <unistd.h>
|
||||
#include <stdint.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/timerfd.h>
|
||||
|
||||
//时间戳 微秒级， 需要#include <sys/time.h>
|
||||
inline long long getSystemTime(){
|
||||
struct timeval t;
|
||||
gettimeofday(&t, NULL);
|
||||
return 1000000 * t.tv_sec + t.tv_usec;
|
||||
}
|
||||
//时间戳 秒级， 需要getSystemTime()
|
||||
inline double getTimeSecond(){
|
||||
double time = getSystemTime() * 0.000001;
|
||||
return time;
|
||||
}
|
||||
/*
|
||||
等待函数，微秒级，从startTime开始等待waitTime微秒
|
||||
目前不推荐使用，建议使用AbsoluteTimer类
|
||||
*/
|
||||
inline void absoluteWait(long long startTime, long long waitTime){
|
||||
if(getSystemTime() - startTime > waitTime){
|
||||
std::cout << "[WARNING] The waitTime=" << waitTime << " of function absoluteWait is not enough!" << std::endl
|
||||
<< "The program has already cost " << getSystemTime() - startTime << "us." << std::endl;
|
||||
}
|
||||
while(getSystemTime() - startTime < waitTime){
|
||||
usleep(50);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
waitTimeS = 0 means do not care time out
|
||||
*/
|
||||
class AbsoluteTimer{
|
||||
public:
|
||||
AbsoluteTimer(double waitTimeS);
|
||||
~AbsoluteTimer();
|
||||
void start();
|
||||
bool wait();
|
||||
private:
|
||||
void _updateWaitTime(double waitTimeS);
|
||||
int _timerFd;
|
||||
uint64_t _missed;
|
||||
double _waitTime;
|
||||
double _startTime;
|
||||
double _leftTime;
|
||||
double _nextWaitTime;
|
||||
itimerspec _timerSpec;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,66 @@
|
|||
#ifndef _UNITREE_ARM_UDP_H_
|
||||
#define _UNITREE_ARM_UDP_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <unistd.h>
|
||||
#include <vector>
|
||||
#include <arpa/inet.h>
|
||||
#include <string>
|
||||
#include <string.h>
|
||||
#include <iostream>
|
||||
#include <unistd.h>
|
||||
#include <termios.h>
|
||||
|
||||
#include "unitree_arm_sdk/common/arm_motor_common.h"
|
||||
|
||||
enum class BlockYN{
|
||||
YES,
|
||||
NO
|
||||
};
|
||||
|
||||
class IOPort{
|
||||
public:
|
||||
IOPort(BlockYN blockYN, size_t recvLength, size_t timeOutUs){
|
||||
resetIO(blockYN, recvLength, timeOutUs);
|
||||
}
|
||||
virtual ~IOPort(){}
|
||||
virtual size_t send(uint8_t *sendMsg, size_t sendLength) = 0;
|
||||
virtual size_t recv(uint8_t *recvMsg, size_t recvLength) = 0;
|
||||
virtual size_t recv(uint8_t *recvMsg) = 0;
|
||||
virtual bool sendRecv(std::vector<MOTOR_send> &sendVec, std::vector<MOTOR_recv> &recvVec) = 0;
|
||||
void resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs);
|
||||
protected:
|
||||
BlockYN _blockYN = BlockYN::NO;
|
||||
size_t _recvLength;
|
||||
timeval _timeout;
|
||||
timeval _timeoutSaved;
|
||||
};
|
||||
|
||||
|
||||
class UDPPort : public IOPort{
|
||||
public:
|
||||
UDPPort(std::string toIP, uint toPort, uint ownPort,
|
||||
size_t recvLength = 0,
|
||||
BlockYN blockYN = BlockYN::NO,
|
||||
size_t timeOutUs = 20000);
|
||||
~UDPPort();
|
||||
size_t send(uint8_t *sendMsg, size_t sendMsgLength);
|
||||
size_t recv(uint8_t *recvMsg, size_t recvLength);
|
||||
size_t recv(uint8_t *recvMsg);
|
||||
bool sendRecv(std::vector<MOTOR_send> &sendVec, std::vector<MOTOR_recv> &recvVec);
|
||||
private:
|
||||
size_t _recvBlock(uint8_t *recvMsg, size_t recvLength);
|
||||
size_t _recvUnBlock(uint8_t *recvMsg, size_t recvLength);
|
||||
sockaddr_in _ownAddr, _toAddr, _fromAddr;
|
||||
socklen_t _sockaddrSize;
|
||||
int _sockfd;
|
||||
int _on = 1;
|
||||
size_t _sentLength;
|
||||
|
||||
uint8_t _sendBytes[238]; // 7 motors
|
||||
uint8_t _recvBytes[546]; // 7 motors
|
||||
|
||||
fd_set _rSet;
|
||||
};
|
||||
|
||||
#endif // _UNITREE_ARM_UDP_H_
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
#ifndef _UNITREE_ARM_SDK_H_
|
||||
#define _UNITREE_ARM_SDK_H_
|
||||
|
||||
#include "unitree_arm_sdk/timer.h"
|
||||
#include "unitree_arm_sdk/common/arm_motor_common.h"
|
||||
#include "unitree_arm_sdk/common/arm_common.h"
|
||||
#include "unitree_arm_sdk/common/quadruped_common.h"
|
||||
#include "unitree_arm_sdk/udp.h"
|
||||
#include "unitree_arm_sdk/loop.h"
|
||||
#include "unitree_arm_sdk/joystick.h"
|
||||
#include "unitree_arm_sdk/keyboard.h"
|
||||
|
||||
#endif
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Loading…
Reference in New Issue