EmbodiedAI
/
z1_sdk
mirror of https://github.com/unitreerobotics/z1_sdk.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

V1.1.0 

1. 增添两种跟踪模式下的示例代码
This commit is contained in:
Agnel Wang 2022-09-19 17:08:57 +08:00
parent 0b7b40d4d0
commit 7b45bb38ff
9 changed files with 121 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
CMakeLists.txt
View File

@ -11,9 +11,10 @@ include_directories(
link_directories(lib)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3 -pthread")
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)
add_executable(example_lowCmd_send examples/example_lowCmd_send.cpp )
target_link_libraries(example_lowCmd_send Z1_SDK_Linux64)
add_executable(example_keyboard_send examples/example_keyboard_send.cpp)
@ -22,5 +23,5 @@ 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)
add_executable(getJointGripperState examples/getJointGripperState.cpp )
target_link_libraries(getJointGripperState Z1_SDK_Linux64)

136
examples/bigDemo.cpp
View File

@ -1,119 +1,173 @@
#include "unitreeArm.h"
class Custom : public unitreeArm{
class Z1ARM : public unitreeArm{
public:
Custom(){};
~Custom(){};
Z1ARM(){};
~Z1ARM(){};
void armCtrlByFSM();
void armCtrlByTraj();
void armCtrlTrackInJointCtrl();
void armCtrlTrackInCartesian();
};
void Custom::armCtrlByFSM() {
void Z1ARM::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(){
void Z1ARM::armCtrlByTraj(){
Vec6 posture[2];
std::cout << "[TOSTATE] forward" << std::endl;
labelRun("forward");
sleep(1);
int order=1;
labelRun("forward");
// No.1 trajectory
_trajCmd.trajOrder = order++;
_trajCmd.trajType = TrajType::MoveJ;
_trajCmd.maxSpeed = 0.3;
_trajCmd.gripperPos = -1.0;
_trajCmd.maxSpeed = 1.0;// angular velocity
_trajCmd.gripperPos = 0.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);
usleep(10000);
// No.2 trajectory
_trajCmd.trajOrder = order++;
_trajCmd.trajType = TrajType::Stop;
_trajCmd.stopTime = 1.0;
_trajCmd.gripperPos = -1.0;
setTraj(_trajCmd);
usleep(10000);
// No.3 trajectory
_trajCmd.trajOrder = order++;
_trajCmd.trajType = TrajType::MoveL;
_trajCmd.maxSpeed = 0.2;
_trajCmd.maxSpeed = 0.3; // Cartesian velocity
_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);
usleep(10000);
// No.4 trajectory
_trajCmd.trajOrder = order++;
_trajCmd.trajType = TrajType::Stop;
_trajCmd.stopTime = 2.0;
_trajCmd.gripperPos = 0.0;
_trajCmd.stopTime = 1.0;
_trajCmd.gripperPos = -1.0;
setTraj(_trajCmd);
usleep(500000);
usleep(10000);
// No.4 trajectory
// No.5 trajectory
_trajCmd.trajOrder = order++;
_trajCmd.trajType = TrajType::MoveC;
_trajCmd.maxSpeed = 0.3;
_trajCmd.gripperPos = -1.0;
_trajCmd.maxSpeed = 0.3; // Cartesian velocity
_trajCmd.gripperPos = 0.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);
usleep(10000);
//run
//run trajectory
setFsm(ArmFSMState::TRAJECTORY);
// wait for completion
// wait for trajectory completion
while (_recvState.state != ArmFSMState::JOINTCTRL){
usleep(4000);
}
}
void Z1ARM::armCtrlTrackInJointCtrl(){
labelRun("forward");// auto change to JOINTCTRL state after TOSTATE
for( size_t i(0); i < 6; i++ ){// the current joint cmd must update to be consistent with the state
_sendCmd.valueUnion.jointCmd[i].Pos = _recvState.jointState[i].Pos;
_sendCmd.valueUnion.jointCmd[i].W = 0.0;
}
// while track is true, the arm will track joint cmd (Only q and dq)
_sendCmd.track = true;
for(;;){
_sendCmd.valueUnion.jointCmd[0].Pos += 0.002;
//The joint has reached limit, there is warning: joint cmd is far from state
double error = abs(_sendCmd.valueUnion.jointCmd[0].Pos - _recvState.jointState[0].Pos);
if(error > 0.1){
break;
}
usleep(4000);
}
_sendCmd.track = false;
}
void Z1ARM::armCtrlTrackInCartesian(){
labelRun("forward");
setFsm(ArmFSMState::CARTESIAN);
// the current posture cmd must update to be consistent with the state
_sendCmd.valueUnion.trajCmd.posture[0] = _recvState.cartesianState;
// while track is true, the arm will track posture[0] cmd
_sendCmd.track = true;
for(;;){
_sendCmd.valueUnion.trajCmd.posture[0].y += 0.0005;
// std::cout << PosturetoVec6(_sendCmd.valueUnion.trajCmd.posture[0]).transpose() << std::endl;
// no inverse kinematics solution, the joint has reached limit
double error = (PosturetoVec6(_recvState.cartesianState) - PosturetoVec6(_sendCmd.valueUnion.trajCmd.posture[0])).norm();
if( error > 0.1){
break;
}
usleep(4000);
}
_sendCmd.track = false;
}
int main() {
Custom custom;
Z1ARM arm;
custom.backToStart();
arm.backToStart();
sleep(1);
if(false){
custom.armCtrlByFSM();
}else{
custom.armCtrlByTraj();
size_t demo = 2;
// for(size_t demo = 1; demo < 5; demo++)
// for(;;)
switch (demo)
{
case 1:
arm.armCtrlByFSM();
break;
case 2:
arm.armCtrlByTraj();
break;
case 3:
arm.armCtrlTrackInJointCtrl();
break;
case 4:
arm.armCtrlTrackInCartesian();
break;
default:
break;
}
custom.backToStart();
arm.backToStart();
return 0;
}

9
include/unitree_arm_sdk/common/aliengo_common.h
View File

@ -13,6 +13,7 @@ namespace UNITREE_LEGGED_SDK
#pragma pack(1)
// 12 bytes
typedef struct
{
float x;
@ -20,6 +21,7 @@ namespace UNITREE_LEGGED_SDK
float z;
} Cartesian;
// 53 bytes
typedef struct
{
float quaternion[4]; // quaternion, normalized, (w,x,y,z)
@ -29,6 +31,7 @@ namespace UNITREE_LEGGED_SDK
int8_t temperature;
} IMU; // when under accelerated motion, the attitude of the robot calculated by IMU will drift.
// 3 bytes
typedef struct
{
uint8_t r;
@ -36,6 +39,7 @@ namespace UNITREE_LEGGED_SDK
uint8_t b;
} LED; // foot led brightness: 0~255
// 38 bytes
typedef struct
{
uint8_t mode; // motor working mode
@ -50,6 +54,7 @@ namespace UNITREE_LEGGED_SDK
uint32_t reserve[2]; // in reserve[1] returns the brake state.
} MotorState; // motor feedback
// 33 bytes
typedef struct
{
uint8_t mode; // desired working mode
@ -61,6 +66,7 @@ namespace UNITREE_LEGGED_SDK
uint32_t reserve[3]; // in reserve[0] sends the brake cmd.
} MotorCmd; // motor control
// 891 bytes
typedef struct
{
uint8_t levelFlag; // flag to distinguish high level or low level
@ -78,6 +84,7 @@ namespace UNITREE_LEGGED_SDK
uint32_t crc;
} LowState; // low level feedback
// 730 bytes
typedef struct
{
uint8_t levelFlag;
@ -92,6 +99,7 @@ namespace UNITREE_LEGGED_SDK
uint32_t crc;
} LowCmd; // low level control
// 244byte
typedef struct
{
uint8_t levelFlag;
@ -112,6 +120,7 @@ namespace UNITREE_LEGGED_SDK
uint32_t crc;
} HighState; // high level feedback
//113byte
typedef struct
{
uint8_t levelFlag;

5
include/unitree_arm_sdk/common/arm_common.h
View File

@ -69,13 +69,13 @@ struct JointCmd{
// 16 Byte
struct JointState{
float buf[0];
float T;
float W;
float Acc;
float Pos;
};
//140bytes
union UDPSendCmd{
uint8_t checkCmd;
JointCmd jointCmd[7];
@ -88,8 +88,6 @@ union UDPRecvState{
uint8_t errorCheck[16];
};
// 24 Byte
struct Posture{
double roll;
double pitch;
@ -118,6 +116,7 @@ struct SendCmd{
uint8_t head[2];
ArmFSMState state;
ArmFSMValue value;
bool track;// whether let arm track jointCmd in State_JOINTCTRL or posture[0] in State_CARTESIAN
ValueUnion valueUnion;
};

2
include/unitree_arm_sdk/joystick.h
View File

@ -23,7 +23,9 @@ public:
EmptyAction emptyAction, size_t channelNum = 1,
double dt = 0.002);
~UnitreeJoystick();
SendCmd getSendCmd(){return _sendCmd;};
private:
SendCmd _sendCmd;
void _read();
void _extractCmd();

BIN
lib/libZ1_SDK_Linux64.so
View File

Binary file not shown.

BIN
thirdparty/lcm-1.4.0.zip vendored
View File

Binary file not shown.

7
unitreeArm/unitreeArm.cpp
View File

@ -3,8 +3,10 @@
// 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 = {0};
_sendCmd.head[0] = 0xFE;
_sendCmd.head[1] = 0xFF;
_sendCmd.track = false;
_udpThread = new LoopFunc("udp", 0.004, boost::bind(&unitreeArm::UDPSendRecv, this));
_udpThread->start();
@ -15,7 +17,7 @@ unitreeArm::~unitreeArm() {
}
// udp send and receive function
// it has created a thread in the constructor that used to automatically process the arm state and command datas
// 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));
@ -34,6 +36,7 @@ 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);
}
@ -79,7 +82,7 @@ void unitreeArm::MoveJ(Vec6 moveJCmd) {
// reach the target posture by directly moving the joint
void unitreeArm::MoveJ(Vec6 moveJCmd, double gripperPos) {
_sendCmd.valueUnion.jointCmd[6].Pos = gripperPos;
_sendCmd.valueUnion.trajCmd.gripperPos = gripperPos;
MoveJ(moveJCmd);
}