diff --git a/CMakeLists.txt b/CMakeLists.txt index dbf7a00..57c0ee7 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) -target_link_libraries(getJointGripperState Z1_SDK_Linux64) +add_executable(getJointGripperState examples/getJointGripperState.cpp ) +target_link_libraries(getJointGripperState Z1_SDK_Linux64) \ No newline at end of file diff --git a/examples/bigDemo.cpp b/examples/bigDemo.cpp index 878d7b0..ccec29c 100644 --- a/examples/bigDemo.cpp +++ b/examples/bigDemo.cpp @@ -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(); - } - - custom.backToStart(); + 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; + } + + arm.backToStart(); return 0; } \ No newline at end of file diff --git a/include/unitree_arm_sdk/common/aliengo_common.h b/include/unitree_arm_sdk/common/aliengo_common.h index 7a916d2..4783e9e 100644 --- a/include/unitree_arm_sdk/common/aliengo_common.h +++ b/include/unitree_arm_sdk/common/aliengo_common.h @@ -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; diff --git a/include/unitree_arm_sdk/common/arm_common.h b/include/unitree_arm_sdk/common/arm_common.h index bc39120..675a1bc 100755 --- a/include/unitree_arm_sdk/common/arm_common.h +++ b/include/unitree_arm_sdk/common/arm_common.h @@ -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; }; diff --git a/include/unitree_arm_sdk/joystick.h b/include/unitree_arm_sdk/joystick.h index 57690c3..834a9f0 100755 --- a/include/unitree_arm_sdk/joystick.h +++ b/include/unitree_arm_sdk/joystick.h @@ -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(); diff --git a/lib/libZ1_SDK_Linux64.so b/lib/libZ1_SDK_Linux64.so index d5a3308..08122c1 100755 Binary files a/lib/libZ1_SDK_Linux64.so and b/lib/libZ1_SDK_Linux64.so differ diff --git a/thirdparty/lcm-1.4.0.zip b/thirdparty/lcm-1.4.0.zip deleted file mode 100644 index ab421d7..0000000 Binary files a/thirdparty/lcm-1.4.0.zip and /dev/null differ diff --git a/unitreeArm/unitreeArm.cpp b/unitreeArm/unitreeArm.cpp index a1cd86c..c8e3860 100644 --- a/unitreeArm/unitreeArm.cpp +++ b/unitreeArm/unitreeArm.cpp @@ -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); } diff --git a/unitreeArm/unitreeArm.h b/unitreeArm/unitreeArm.h index 2141ab8..7a26dd1 100644 --- a/unitreeArm/unitreeArm.h +++ b/unitreeArm/unitreeArm.h @@ -55,4 +55,4 @@ private: LoopFunc *_udpThread; }; -#endif \ No newline at end of file +#endif