V1.1.0

1. 增添两种跟踪模式下的示例代码

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)
+add_executable(getJointGripperState examples/getJointGripperState.cpp )
-target_link_libraries(getJointGripperState Z1_SDK_Linux64)
+target_link_libraries(getJointGripperState Z1_SDK_Linux64)

examples/bigDemo.cpp

@@ -1,119 +1,173 @@
 #include "unitreeArm.h"
 
-class Custom : public unitreeArm{
+class Z1ARM : public unitreeArm{
 
 public:
-    Custom(){};
+    Z1ARM(){};
-    ~Custom(){};
+    ~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.maxSpeed = 1.0;// angular velocity
-    _trajCmd.gripperPos = -1.0;
+    _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.stopTime = 1.0; 
-    _trajCmd.gripperPos = 0.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.maxSpeed = 0.3; // Cartesian velocity
-    _trajCmd.gripperPos = -1.0;
+    _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){
+    size_t demo = 2;
-        custom.armCtrlByFSM();
+    // for(size_t demo = 1; demo < 5; demo++)
-    }else{
+    // for(;;)
-        custom.armCtrlByTraj();
+        switch (demo)
-    }
+        {
-
+            case 1:
-    custom.backToStart();
+                arm.armCtrlByFSM();
+                break;
+            case 2:
+                arm.armCtrlByTraj();
+                break;
+            case 3:
+                arm.armCtrlTrackInJointCtrl();
+                break;
+            case 4:
+                arm.armCtrlTrackInCartesian();
+                break;
+            default:
+                break;
+        }
+    
+    arm.backToStart();
     return 0;
 }

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;

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;
 };
 

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();
 

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);
 }
 

unitreeArm/unitreeArm.h

@@ -55,4 +55,4 @@ private:
     LoopFunc *_udpThread;
 };
 
-#endif
+#endif