first commit
This commit is contained in:
Agnel Wang
parent
5221e95827
commit
0b7b40d4d0
|
|
@ -1 +0,0 @@
|
|||
build
|
||||
|
|
@ -5,6 +5,7 @@ project(z1_sdk)
|
|||
|
||||
include_directories(
|
||||
include
|
||||
unitreeArm
|
||||
# ${EIGEN_PATH}
|
||||
)
|
||||
|
||||
|
|
@ -12,24 +13,14 @@ 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)
|
||||
|
||||
add_executable(bigDemo examples/bigDemo.cpp)
|
||||
target_link_libraries(bigDemo Z1_SDK_Linux64)
|
||||
|
||||
add_executable(getJointGripperState examples/getJointGripperState.cpp)
|
||||
target_link_libraries(getJointGripperState Z1_SDK_Linux64)
|
||||
|
|
|
|||
77
README.md
77
README.md
|
|
@ -1,76 +1 @@
|
|||
# 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
|
||||
```
|
||||
see [unitree-z1-docs](http://dev-z1.unitree.com/)
|
||||
|
|
@ -0,0 +1,119 @@
|
|||
#include "unitreeArm.h"
|
||||
|
||||
class Custom : public unitreeArm{
|
||||
|
||||
public:
|
||||
Custom(){};
|
||||
~Custom(){};
|
||||
void armCtrlByFSM();
|
||||
void armCtrlByTraj();
|
||||
};
|
||||
|
||||
|
||||
void Custom::armCtrlByFSM() {
|
||||
Vec6 posture[2];
|
||||
|
||||
std::cout << "[JOINTCTRL]" << std::endl;
|
||||
setFsm(ArmFSMState::JOINTCTRL);
|
||||
sleep(1);// wait for a while for the command to execute
|
||||
|
||||
std::cout << "[TOSTATE] forward" << std::endl;
|
||||
labelRun("forward");
|
||||
sleep(1);
|
||||
|
||||
std::cout << "[MOVEJ]" << std::endl;
|
||||
posture[0]<<0.5,0.1,0.1,0.5,-0.2,0.5;
|
||||
MoveJ(posture[0], -1.0);
|
||||
sleep(1);
|
||||
|
||||
std::cout << "[MOVEL]" << std::endl;
|
||||
posture[0] << 0,0,0,0.45,-0.2,0.2;
|
||||
MoveL(posture[0]);
|
||||
sleep(1);
|
||||
|
||||
std::cout << "[MOVEC]" << std::endl;
|
||||
posture[0] << 0,0,0,0.4,0,0.3;
|
||||
posture[1] << 0,0,0,0.45,0.2,0.2;
|
||||
MoveC(posture[0], posture[1]);
|
||||
sleep(1);
|
||||
|
||||
std::cout << "[BACKTOSTART]" << std::endl;
|
||||
backToStart();
|
||||
sleep(1);
|
||||
}
|
||||
|
||||
void Custom::armCtrlByTraj(){
|
||||
Vec6 posture[2];
|
||||
|
||||
std::cout << "[TOSTATE] forward" << std::endl;
|
||||
labelRun("forward");
|
||||
sleep(1);
|
||||
|
||||
int order=1;
|
||||
|
||||
// No.1 trajectory
|
||||
_trajCmd.trajOrder = order++;
|
||||
_trajCmd.trajType = TrajType::MoveJ;
|
||||
_trajCmd.maxSpeed = 0.3;
|
||||
_trajCmd.gripperPos = -1.0;
|
||||
posture[0] << 0.5,0.1,0.1,0.5,-0.2,0.5;
|
||||
// posture[0] << 0.0,0.0,0,0.5,-0.2,0.5;
|
||||
_trajCmd.posture[0] = Vec6toPosture(posture[0]);
|
||||
setTraj(_trajCmd);
|
||||
usleep(500000);
|
||||
|
||||
// No.2 trajectory
|
||||
_trajCmd.trajOrder = order++;
|
||||
_trajCmd.trajType = TrajType::MoveL;
|
||||
_trajCmd.maxSpeed = 0.2;
|
||||
_trajCmd.gripperPos = 0.0;
|
||||
posture[0] << 0,0,0,0.45,-0.2,0.2;
|
||||
_trajCmd.posture[0] = Vec6toPosture(posture[0]);
|
||||
setTraj(_trajCmd);
|
||||
usleep(500000);
|
||||
|
||||
// No.4 trajectory
|
||||
_trajCmd.trajOrder = order++;
|
||||
_trajCmd.trajType = TrajType::Stop;
|
||||
_trajCmd.stopTime = 2.0;
|
||||
_trajCmd.gripperPos = 0.0;
|
||||
setTraj(_trajCmd);
|
||||
usleep(500000);
|
||||
|
||||
// No.4 trajectory
|
||||
_trajCmd.trajOrder = order++;
|
||||
_trajCmd.trajType = TrajType::MoveC;
|
||||
_trajCmd.maxSpeed = 0.3;
|
||||
_trajCmd.gripperPos = -1.0;
|
||||
posture[0] << 0,0,0,0.45,0,0.4;
|
||||
posture[1] << 0,0,0,0.45,0.2,0.2;
|
||||
_trajCmd.posture[0] = Vec6toPosture(posture[0]);
|
||||
_trajCmd.posture[1] = Vec6toPosture(posture[1]);
|
||||
setTraj(_trajCmd);
|
||||
usleep(500000);
|
||||
|
||||
//run
|
||||
setFsm(ArmFSMState::TRAJECTORY);
|
||||
|
||||
// wait for completion
|
||||
while (_recvState.state != ArmFSMState::JOINTCTRL){
|
||||
usleep(4000);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int main() {
|
||||
Custom custom;
|
||||
|
||||
custom.backToStart();
|
||||
sleep(1);
|
||||
|
||||
if(false){
|
||||
custom.armCtrlByFSM();
|
||||
}else{
|
||||
custom.armCtrlByTraj();
|
||||
}
|
||||
|
||||
custom.backToStart();
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,53 +0,0 @@
|
|||
/*****************************************************************
|
||||
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;
|
||||
}
|
||||
|
|
@ -1,27 +0,0 @@
|
|||
/*****************************************************************
|
||||
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;
|
||||
}
|
||||
|
|
@ -1,219 +0,0 @@
|
|||
/*****************************************************************
|
||||
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,50 @@
|
|||
#include "unitreeArm.h"
|
||||
|
||||
class Custom : public unitreeArm {
|
||||
public:
|
||||
Custom(){};
|
||||
~Custom(){};
|
||||
void printJointGripperState();
|
||||
Posture _gripperState;
|
||||
};
|
||||
|
||||
void Custom::printJointGripperState() {
|
||||
getJointState(_jointState);
|
||||
std::cout<<std::endl;
|
||||
std::cout<<"------ joint Pos ------"<<std::endl;
|
||||
std::cout<<"joint0: "<<_jointState[0].Pos<<std::endl;
|
||||
std::cout<<"joint1: "<<_jointState[1].Pos<<std::endl;
|
||||
std::cout<<"joint2: "<<_jointState[2].Pos<<std::endl;
|
||||
std::cout<<"joint3: "<<_jointState[3].Pos<<std::endl;
|
||||
std::cout<<"joint4: "<<_jointState[4].Pos<<std::endl;
|
||||
std::cout<<"joint5: "<<_jointState[5].Pos<<std::endl;
|
||||
std::cout<<"------ joint Pos ------"<<std::endl;
|
||||
std::cout<<std::endl;
|
||||
|
||||
getGripperState(_gripperState);
|
||||
std::cout<<std::endl;
|
||||
std::cout<<"------ GripperState ------"<<std::endl;
|
||||
std::cout<<"roll: "<<_gripperState.roll<<std::endl;
|
||||
std::cout<<"pitch: "<<_gripperState.pitch<<std::endl;
|
||||
std::cout<<"yaw: "<<_gripperState.yaw<<std::endl;
|
||||
std::cout<<"x: "<<_gripperState.x<<std::endl;
|
||||
std::cout<<"y: "<<_gripperState.y<<std::endl;
|
||||
std::cout<<"z: "<<_gripperState.z<<std::endl;
|
||||
std::cout<<"------ GripperState Pos ------"<<std::endl;
|
||||
std::cout<<std::endl;
|
||||
|
||||
}
|
||||
|
||||
int main() {
|
||||
|
||||
Custom custom;
|
||||
LoopFunc loop_udpSendRecv("udp", 0.002, boost::bind(&Custom::UDPSendRecv,&custom));
|
||||
LoopFunc loop_printJointState("printJointState", 1, boost::bind(&Custom::printJointGripperState,&custom));
|
||||
loop_udpSendRecv.start();
|
||||
loop_printJointState.start();
|
||||
|
||||
while (1) {
|
||||
|
||||
usleep(1000000);
|
||||
}
|
||||
}
|
||||
|
|
@ -77,7 +77,7 @@ public:
|
|||
void setDt(double dt);
|
||||
double getValue();
|
||||
double getDValue();
|
||||
double setValue(double value){_value = value;}
|
||||
void setValue(double value){_value = value;}
|
||||
private:
|
||||
double _value;
|
||||
double _changeDirection;
|
||||
|
|
@ -112,6 +112,7 @@ public:
|
|||
virtual SendCmd getSendCmd();
|
||||
virtual void getRecvState(RecvState recvState);
|
||||
|
||||
bool isDisConnect;
|
||||
protected:
|
||||
virtual void _read() = 0;
|
||||
void _start();
|
||||
|
|
@ -120,9 +121,6 @@ protected:
|
|||
void _pressKeyboard();
|
||||
void _releaseKeyboard();
|
||||
|
||||
static void* _runStatic(void* obj);
|
||||
static void* _readStatic(void* obj);
|
||||
|
||||
LoopFunc *_runThread;
|
||||
LoopFunc *_readThread;
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,159 @@
|
|||
#ifndef _UNITREE_ARM_ALIENGO_COMMON_H_
|
||||
#define _UNITREE_ARM_ALIENGO_COMMON_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
namespace UNITREE_LEGGED_SDK
|
||||
{
|
||||
|
||||
constexpr int HIGHLEVEL = 0x00;
|
||||
constexpr int LOWLEVEL = 0xff;
|
||||
constexpr double PosStopF = (2.146E+9f);
|
||||
constexpr double VelStopF = (16000.0f);
|
||||
|
||||
#pragma pack(1)
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
} Cartesian;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float quaternion[4]; // quaternion, normalized, (w,x,y,z)
|
||||
float gyroscope[3]; // angular velocity （unit: rad/s)
|
||||
float accelerometer[3]; // m/(s2)
|
||||
float rpy[3]; // 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)
|
||||
uint32_t reserve[2]; // in reserve[1] returns the brake state.
|
||||
} 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) )
|
||||
uint32_t reserve[3]; // in reserve[0] sends the brake cmd.
|
||||
} MotorCmd; // motor control
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t levelFlag; // flag to distinguish high level or low level
|
||||
uint16_t commVersion;
|
||||
uint16_t robotID;
|
||||
uint32_t SN;
|
||||
uint8_t bandWidth;
|
||||
IMU imu;
|
||||
MotorState motorState[20];
|
||||
int16_t footForce[4]; // force sensors
|
||||
int16_t footForceEst[4]; // force sensors
|
||||
uint32_t tick; // reference real-time from motion controller (unit: us)
|
||||
uint8_t wirelessRemote[40]; // wireless commands
|
||||
uint32_t reserve;
|
||||
uint32_t crc;
|
||||
} LowState; // low level feedback
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t levelFlag;
|
||||
uint16_t commVersion;
|
||||
uint16_t robotID;
|
||||
uint32_t SN;
|
||||
uint8_t bandWidth;
|
||||
MotorCmd motorCmd[20];
|
||||
LED led[4];
|
||||
uint8_t wirelessRemote[40];
|
||||
uint32_t reserve;
|
||||
uint32_t crc;
|
||||
} LowCmd; // low level control
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t levelFlag;
|
||||
uint16_t commVersion;
|
||||
uint16_t robotID;
|
||||
uint32_t SN;
|
||||
uint8_t bandWidth;
|
||||
uint8_t mode;
|
||||
IMU imu;
|
||||
float position[3]; // (unit: m), from robot own odometry in inertial frame, usually drift
|
||||
float velocity[3]; // (unit: m/s), forwardSpeed, sideSpeed, updownSpeed in body frame
|
||||
float yawSpeed; // (unit: rad/s), rotateSpeed in body frame.
|
||||
Cartesian footPosition2Body[4]; // foot position relative to body
|
||||
Cartesian footSpeed2Body[4]; // foot speed relative to body
|
||||
int16_t footForce[4];
|
||||
uint8_t wirelessRemote[40];
|
||||
uint32_t reserve;
|
||||
uint32_t crc;
|
||||
} HighState; // high level feedback
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t levelFlag;
|
||||
uint16_t commVersion;
|
||||
uint16_t robotID;
|
||||
uint32_t SN;
|
||||
uint8_t bandWidth;
|
||||
uint8_t mode; // 0.idle, default stand | 1.force stand (controlled by dBodyHeight + rpy)
|
||||
// 2.target velocity walking (controlled by velocity + yawSpeed)
|
||||
// 3.target position walking (controlled by position + rpy[2])
|
||||
// 4. path mode walking (reserve for future release)
|
||||
// 5. position stand down. |6. position stand up |7. damping mode | 8. recovery mode
|
||||
uint8_t gaitType; // 0.trot | 1. trot running | 2.climb stair
|
||||
uint8_t speedLevel; // 0. default low speed. 1. medium speed 2. high speed. during walking
|
||||
float dFootRaiseHeight; // (unit: m), swing foot height adjustment from default swing height.
|
||||
float dBodyHeight; // (unit: m), body height adjustment from default body height.
|
||||
float position[2]; // (unit: m), desired x and y position in inertial frame.
|
||||
float rpy[3]; // (unit: rad), desired yaw-pitch-roll euler angle, expressed in roll(rpy[0]) pitch(rpy[1]) yaw(rpy[2])
|
||||
float velocity[2]; // (unit: m/s), forwardSpeed, sideSpeed in body frame.
|
||||
float yawSpeed; // (unit: rad/s), rotateSpeed in body frame.
|
||||
LED led[4];
|
||||
uint8_t wirelessRemote[40];
|
||||
uint32_t reserve;
|
||||
uint32_t crc;
|
||||
} HighCmd; // high level control uint8_t mode
|
||||
|
||||
#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
|
||||
|
||||
constexpr int HIGH_CMD_LENGTH = (sizeof(HighCmd));
|
||||
constexpr int HIGH_STATE_LENGTH = (sizeof(HighState));
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -20,6 +20,7 @@ enum class ArmFSMState{
|
|||
TEACH,
|
||||
TEACHREPEAT,
|
||||
CALIBRATION,
|
||||
SETTRAJ,
|
||||
DANCE00,
|
||||
DANCE01,
|
||||
DANCE02,
|
||||
|
|
@ -50,6 +51,13 @@ enum class ArmFSMValue{
|
|||
UP
|
||||
};
|
||||
|
||||
enum class TrajType{
|
||||
MoveJ,
|
||||
MoveL,
|
||||
MoveC,
|
||||
Stop
|
||||
};
|
||||
|
||||
// 20 Byte
|
||||
struct JointCmd{
|
||||
float T;
|
||||
|
|
@ -61,55 +69,51 @@ struct JointCmd{
|
|||
|
||||
// 16 Byte
|
||||
struct JointState{
|
||||
float buf[0];
|
||||
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;
|
||||
double roll;
|
||||
double pitch;
|
||||
double yaw;
|
||||
double x;
|
||||
double y;
|
||||
double z;
|
||||
};
|
||||
|
||||
// 48 Byte
|
||||
struct MoveC{
|
||||
Posture middlePosture;
|
||||
Posture endPosture;
|
||||
struct TrajCmd{
|
||||
TrajType trajType;
|
||||
Posture posture[2];
|
||||
double gripperPos;
|
||||
double maxSpeed;
|
||||
double stopTime;
|
||||
int trajOrder;
|
||||
};
|
||||
|
||||
// 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];
|
||||
char name[10];
|
||||
JointCmd jointCmd[7];
|
||||
TrajCmd trajCmd;
|
||||
};
|
||||
|
||||
// 2+4+4+140 = 150 Byte
|
||||
struct SendCmd{
|
||||
uint8_t head[2];
|
||||
ArmFSMState state;
|
||||
|
|
@ -117,7 +121,6 @@ struct SendCmd{
|
|||
ValueUnion valueUnion;
|
||||
};
|
||||
|
||||
// 2+4+112+24 = 142 Byte
|
||||
struct RecvState{
|
||||
uint8_t head[2];
|
||||
ArmFSMState state;
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
#ifndef _UNITREE_ARM_QUADRUPED_COMMON_H_
|
||||
#define _UNITREE_ARM_QUADRUPED_COMMON_H_
|
||||
#ifndef _UNITREE_ARM_B1_COMMON_H_
|
||||
#define _UNITREE_ARM_B1_COMMON_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <array>
|
||||
|
|
@ -206,7 +206,6 @@ namespace UNITREE_LEGGED_SDK
|
|||
unsigned long long RecvCRCError; // total reveive CRC error
|
||||
unsigned long long RecvLoseError; // total lose package count
|
||||
} UDPState; // UDP communication state
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -4,9 +4,17 @@
|
|||
#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"
|
||||
|
||||
#define CTRL_BY_ALIENGO_JOYSTICK
|
||||
|
||||
#ifdef CTRL_BY_ALIENGO_JOYSTICK
|
||||
#include "unitree_arm_sdk/common/aliengo_common.h"
|
||||
#else
|
||||
#include "unitree_arm_sdk/common/b1_common.h"
|
||||
#endif
|
||||
|
||||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
|
||||
class UnitreeJoystick : public CmdPanel{
|
||||
|
|
|
|||
|
|
@ -0,0 +1,99 @@
|
|||
#ifndef MATHTYPES_H
|
||||
#define MATHTYPES_H
|
||||
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
/************************/
|
||||
/******** Vector ********/
|
||||
/************************/
|
||||
// 2x1 Vector
|
||||
using Vec2 = typename Eigen::Matrix<double, 2, 1>;
|
||||
|
||||
// 3x1 Vector
|
||||
using Vec3 = typename Eigen::Matrix<double, 3, 1>;
|
||||
|
||||
// 4x1 Vector
|
||||
using Vec4 = typename Eigen::Matrix<double, 4, 1>;
|
||||
|
||||
// 6x1 Vector
|
||||
using Vec6 = typename Eigen::Matrix<double, 6, 1>;
|
||||
|
||||
// Quaternion
|
||||
using Quat = typename Eigen::Matrix<double, 4, 1>;
|
||||
|
||||
// 4x1 Integer Vector
|
||||
using VecInt4 = typename Eigen::Matrix<int, 4, 1>;
|
||||
|
||||
// 12x1 Vector
|
||||
using Vec12 = typename Eigen::Matrix<double, 12, 1>;
|
||||
|
||||
// 18x1 Vector
|
||||
using Vec18 = typename Eigen::Matrix<double, 18, 1>;
|
||||
|
||||
// Dynamic Length Vector
|
||||
using VecX = typename Eigen::Matrix<double, Eigen::Dynamic, 1>;
|
||||
|
||||
/************************/
|
||||
/******** Matrix ********/
|
||||
/************************/
|
||||
// Rotation Matrix
|
||||
using RotMat = typename Eigen::Matrix<double, 3, 3>;
|
||||
|
||||
// Homogenous Matrix
|
||||
using HomoMat = typename Eigen::Matrix<double, 4, 4>;
|
||||
|
||||
// 2x2 Matrix
|
||||
using Mat2 = typename Eigen::Matrix<double, 2, 2>;
|
||||
|
||||
// 3x3 Matrix
|
||||
using Mat3 = typename Eigen::Matrix<double, 3, 3>;
|
||||
|
||||
// 3x4 Matrix, each column is a 3x1 vector
|
||||
using Vec34 = typename Eigen::Matrix<double, 3, 4>;
|
||||
|
||||
// 6x6 Matrix
|
||||
using Mat6 = typename Eigen::Matrix<double, 6, 6>;
|
||||
|
||||
// 12x12 Matrix
|
||||
using Mat12 = typename Eigen::Matrix<double, 12, 12>;
|
||||
|
||||
// 3x3 Identity Matrix
|
||||
#define I3 Eigen::MatrixXd::Identity(3, 3)
|
||||
|
||||
// 6x6 Identity Matrix
|
||||
#define I6 Eigen::MatrixXd::Identity(6, 6)
|
||||
|
||||
// 12x12 Identity Matrix
|
||||
#define I12 Eigen::MatrixXd::Identity(12, 12)
|
||||
|
||||
// 18x18 Identity Matrix
|
||||
#define I18 Eigen::MatrixXd::Identity(18, 18)
|
||||
|
||||
// Dynamic Size Matrix
|
||||
using MatX = typename Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
|
||||
|
||||
/************************/
|
||||
/****** Functions *******/
|
||||
/************************/
|
||||
inline Vec34 vec12ToVec34(Vec12 vec12){
|
||||
Vec34 vec34;
|
||||
for(int i(0); i < 4; ++i){
|
||||
vec34.col(i) = vec12.segment(3*i, 3);
|
||||
}
|
||||
return vec34;
|
||||
}
|
||||
|
||||
inline Vec12 vec34ToVec12(Vec34 vec34){
|
||||
Vec12 vec12;
|
||||
for(int i(0); i < 4; ++i){
|
||||
vec12.segment(3*i, 3) = vec34.col(i);
|
||||
}
|
||||
return vec12;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline VecX stdVecToEigenVec(T stdVec){
|
||||
VecX eigenVec = Eigen::VectorXd::Map(&stdVec[0], stdVec.size());
|
||||
return eigenVec;
|
||||
}
|
||||
#endif // MATHTYPES_H
|
||||
|
|
@ -10,6 +10,7 @@
|
|||
#include <iostream>
|
||||
#include <unistd.h>
|
||||
#include <termios.h>
|
||||
#include "unitree_arm_sdk/timer.h"
|
||||
|
||||
#include "unitree_arm_sdk/common/arm_motor_common.h"
|
||||
|
||||
|
|
@ -29,11 +30,13 @@ public:
|
|||
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);
|
||||
bool isDisConnect;
|
||||
protected:
|
||||
BlockYN _blockYN = BlockYN::NO;
|
||||
size_t _recvLength;
|
||||
timeval _timeout;
|
||||
timeval _timeoutSaved;
|
||||
uint16_t _isDisConnectCnt;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -4,10 +4,10 @@
|
|||
#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"
|
||||
#include "mathTypes.h"
|
||||
|
||||
#endif
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1,134 @@
|
|||
#include "unitreeArm.h"
|
||||
|
||||
// constructor
|
||||
// since the SDK communicates with z1_ctrl, the udp is set to the loopback address by default
|
||||
unitreeArm::unitreeArm():_udp("127.0.0.1", 8071, 8072, sizeof(RecvState), BlockYN::NO, 500000) {
|
||||
_sendCmd.head[0] = 0xFE;
|
||||
_sendCmd.head[1] = 0xFF;
|
||||
|
||||
_udpThread = new LoopFunc("udp", 0.004, boost::bind(&unitreeArm::UDPSendRecv, this));
|
||||
_udpThread->start();
|
||||
}
|
||||
|
||||
unitreeArm::~unitreeArm() {
|
||||
delete _udpThread;
|
||||
}
|
||||
|
||||
// udp send and receive function
|
||||
// it has created a thread in the constructor that used to automatically process the arm state and command datas
|
||||
void unitreeArm::UDPSendRecv() {
|
||||
_udp.send((uint8_t*)&_sendCmd, sizeof(SendCmd));
|
||||
_udp.recv((uint8_t*)&_recvState, sizeof(RecvState));
|
||||
}
|
||||
|
||||
// set current FSM to fsm, this is equivalent to key switching when using keyboard control
|
||||
// input: fsm [the specific format can be viewed in class ArmFSMState ]
|
||||
void unitreeArm::setFsm(ArmFSMState fsm) {
|
||||
while (_recvState.state != fsm){
|
||||
_sendCmd.state = fsm;
|
||||
usleep(deltaTime);
|
||||
}
|
||||
}
|
||||
|
||||
void unitreeArm::backToStart() {
|
||||
setFsm(ArmFSMState::BACKTOSTART);
|
||||
//for those states that automatically transion to the joint space state, it's recommmanded to wait for it to finish
|
||||
_sendCmd.state = ArmFSMState::INVALID;
|
||||
while (_recvState.state != ArmFSMState::JOINTCTRL){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
}
|
||||
|
||||
// move the arm to the posture indicated by the label
|
||||
// the specific label name can be viewed in savedArmStates.csv
|
||||
void unitreeArm::labelRun(std::string label) {
|
||||
while (_recvState.state != ArmFSMState::TOSTATE){
|
||||
_sendCmd.state = ArmFSMState::TOSTATE;
|
||||
strcpy(_sendCmd.valueUnion.name, label.c_str());
|
||||
usleep(deltaTime);
|
||||
}
|
||||
_sendCmd.state = ArmFSMState::INVALID;
|
||||
while (_recvState.state != ArmFSMState::JOINTCTRL){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
}
|
||||
|
||||
// save current posture to a label
|
||||
// the label name should be less than 10 chars
|
||||
void unitreeArm::labelSave(std::string label) {
|
||||
_sendCmd.state = ArmFSMState::SAVESTATE;
|
||||
strcpy(_sendCmd.valueUnion.name, label.c_str());
|
||||
while (_recvState.state != ArmFSMState::SAVESTATE){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
}
|
||||
|
||||
// reach the target posture by directly moving the joint
|
||||
void unitreeArm::MoveJ(Vec6 moveJCmd) {
|
||||
_sendCmd.valueUnion.trajCmd.posture[0] = Vec6toPosture(moveJCmd);
|
||||
|
||||
_sendCmd.state = ArmFSMState::MOVEJ;
|
||||
while (_recvState.state != ArmFSMState::MOVEJ){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
_sendCmd.state = ArmFSMState::INVALID;
|
||||
while (_recvState.state != ArmFSMState::JOINTCTRL){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
}
|
||||
|
||||
// reach the target posture by directly moving the joint
|
||||
void unitreeArm::MoveJ(Vec6 moveJCmd, double gripperPos) {
|
||||
_sendCmd.valueUnion.jointCmd[6].Pos = gripperPos;
|
||||
MoveJ(moveJCmd);
|
||||
}
|
||||
|
||||
// the end effector reaches the target point in a straight line trajectory
|
||||
void unitreeArm::MoveL(Vec6 moveLCmd) {
|
||||
_sendCmd.valueUnion.trajCmd.posture[0] = Vec6toPosture(moveLCmd);
|
||||
|
||||
_sendCmd.state = ArmFSMState::MOVEL;
|
||||
while (_recvState.state != ArmFSMState::MOVEL){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
_sendCmd.state = ArmFSMState::INVALID;
|
||||
while (_recvState.state != ArmFSMState::JOINTCTRL){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
}
|
||||
|
||||
// the end effector reaches the target point in a circular arc trajectory
|
||||
void unitreeArm::MoveC(Vec6 middleP, Vec6 endP){
|
||||
_sendCmd.valueUnion.trajCmd.posture[0] = Vec6toPosture(middleP);
|
||||
_sendCmd.valueUnion.trajCmd.posture[1] = Vec6toPosture(endP);
|
||||
|
||||
_sendCmd.state = ArmFSMState::MOVEC;
|
||||
while (_recvState.state != ArmFSMState::MOVEC){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
_sendCmd.state = ArmFSMState::INVALID;
|
||||
while (_recvState.state != ArmFSMState::JOINTCTRL){
|
||||
usleep(deltaTime);
|
||||
}
|
||||
}
|
||||
|
||||
void unitreeArm::getJointState(JointState* jointState) {
|
||||
for(int i=0;i<7;i++) {
|
||||
jointState[i] = _recvState.jointState[i];
|
||||
}
|
||||
}
|
||||
|
||||
void unitreeArm::getGripperState(Posture& gripperState) {
|
||||
gripperState = _recvState.cartesianState;
|
||||
}
|
||||
|
||||
void unitreeArm::setTraj(TrajCmd trajCmd){
|
||||
if(_recvState.state != ArmFSMState::SETTRAJ){
|
||||
_sendCmd.valueUnion.trajCmd.trajOrder = 0;
|
||||
setFsm(ArmFSMState::SETTRAJ);
|
||||
}
|
||||
|
||||
if(_sendCmd.valueUnion.trajCmd.trajOrder == (trajCmd.trajOrder - 1)){// make sure [order] is sequential
|
||||
_sendCmd.valueUnion.trajCmd = trajCmd;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,58 @@
|
|||
#ifndef __UNITREEARM_H
|
||||
#define __UNITREEARM_H
|
||||
|
||||
#include <iostream>
|
||||
#include "unitree_arm_sdk/unitree_arm_sdk.h"
|
||||
|
||||
Vec6 PosturetoVec6(const Posture p){
|
||||
Vec6 posture;
|
||||
posture(0) = p.roll;
|
||||
posture(1) = p.pitch;
|
||||
posture(2) = p.yaw;
|
||||
posture(3) = p.x;
|
||||
posture(4) = p.y;
|
||||
posture(5) = p.z;
|
||||
return posture;
|
||||
}
|
||||
|
||||
Posture Vec6toPosture(const Vec6 p){
|
||||
Posture posture;
|
||||
posture.roll = p(0);
|
||||
posture.pitch = p(1);
|
||||
posture.yaw = p(2);
|
||||
posture.x = p(3);
|
||||
posture.y = p(4);
|
||||
posture.z = p(5);
|
||||
return posture;
|
||||
}
|
||||
|
||||
class unitreeArm{
|
||||
public:
|
||||
unitreeArm();
|
||||
~unitreeArm();
|
||||
void UDPSendRecv();
|
||||
void InitCmdData(SendCmd& sendCmd);
|
||||
void setFsm(ArmFSMState fsm);
|
||||
void backToStart();
|
||||
void labelRun(std::string label);
|
||||
void labelSave(std::string label);
|
||||
void MoveJ(Vec6 moveJCmd);
|
||||
void MoveJ(Vec6 moveJCmd, double gripperPos);
|
||||
void MoveL(Vec6 moveLCmd);
|
||||
void MoveC(Vec6 middleP, Vec6 endP);
|
||||
void getJointState(JointState* jointstate);
|
||||
void getGripperState(Posture& gripperState);
|
||||
void setTraj(TrajCmd trajCmd);
|
||||
|
||||
RecvState _recvState; // the arm state receive from udp
|
||||
JointState _jointState[7]; // 6 joint angles and one gripper angle
|
||||
SendCmd _sendCmd; // command to control the arm
|
||||
TrajCmd _trajCmd;
|
||||
|
||||
private:
|
||||
UDPPort _udp;
|
||||
float deltaTime = 4000; //us
|
||||
LoopFunc *_udpThread;
|
||||
};
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue