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