diff --git a/examples/bigDemo.cpp b/examples/bigDemo.cpp
index db75603..127a677 100644
--- a/examples/bigDemo.cpp
+++ b/examples/bigDemo.cpp
@@ -1,4 +1,4 @@
-#include "control/unitreeArm.h"
+#include "unitree_arm_sdk/control/unitreeArm.h"
 
 using namespace UNITREE_ARM;
 
@@ -63,7 +63,7 @@ void Z1ARM::armCtrlTrackInCartesian(){
         endPosture(5) -= _ctrlComp->dt * 0.2;//z axis, m/s
 
         // no inverse kinematics solution, the joint has reached limit
-        std::cout << "postureCmd: " << endPosture.transpose() << " qState: " << lowstate->endPosture.transpose() << std::endl;
+        // std::cout << "postureCmd: " << endPosture.transpose() << " qState: " << lowstate->endPosture.transpose() << std::endl;
         double error = fabs(endPosture(5) - lowstate->endPosture(5));
         if( error > 0.1){
             break;
@@ -91,7 +91,7 @@ int main() {
 
     arm.backToStart();
 
-    // size_t demo = 3;
+    // size_t demo = 1;
     for(size_t demo = 1; demo < 4; demo++)
     switch (demo)
     {
diff --git a/examples/example_CtrlByFSM.cpp b/examples/example_CtrlByFSM.cpp
index 16afd6f..1eb896e 100644
--- a/examples/example_CtrlByFSM.cpp
+++ b/examples/example_CtrlByFSM.cpp
@@ -1,4 +1,4 @@
-#include "control/unitreeArm.h"
+#include "unitree_arm_sdk/control/unitreeArm.h"
 
 using namespace UNITREE_ARM;
 
@@ -16,17 +16,23 @@ int main() {
     std::cout << "[TO STATE]" << std::endl;
     arm.labelRun("forward");
     
+
     std::cout << "[MOVEJ]" << std::endl;
     posture[0]<<0.5,0.1,0.1,0.5,-0.2,0.5;
     arm.MoveJ(posture[0], -M_PI/3.0, 1.0);
 
     std::cout << "[MOVEL]" << std::endl;
     posture[0] << 0,0,0,0.45,-0.2,0.2;
+    arm.setWait(false);
     arm.MoveL(posture[0], 0., 0.3);
-    
+    while(arm._ctrlComp->recvState.state != ArmFSMState::JOINTCTRL){
+        usleep(2000);
+    }
+
     std::cout << "[MOVEC]" << std::endl;
     posture[0] << 0,0,0,0.45,0,0.4;
     posture[1] << 0,0,0,0.45,0.2,0.2;
+    arm.setWait(true);
     arm.MoveC(posture[0], posture[1], -M_PI/3.0, 0.3);
 
     //stop
diff --git a/examples/example_JointCtrl.cpp b/examples/example_JointCtrl.cpp
index 150ee91..bc3b434 100644
--- a/examples/example_JointCtrl.cpp
+++ b/examples/example_JointCtrl.cpp
@@ -1,4 +1,4 @@
-#include "control/unitreeArm.h"
+#include "unitree_arm_sdk/control/unitreeArm.h"
 
 using namespace UNITREE_ARM;
 
@@ -6,6 +6,8 @@ class JointTraj{
 public:
     JointTraj(){};
     ~JointTraj(){};
+
+    // set _pathTime and a3, a4, a5
     void setJointTraj(Vec6 startQ, Vec6 endQ, double speed){
         _startQ = startQ;
         _endQ = endQ;
@@ -16,6 +18,7 @@ public:
         }
         _generateA345();
     };
+
     void setGripper(double startQ, double endQ, double speed){
         _gripperStartQ = startQ;
         _gripperEndQ = endQ; 
@@ -60,6 +63,10 @@ private:
         _reached = (_tCost>_pathTime) ? true : false;
         _tCost = (_tCost>_pathTime) ? _pathTime : _tCost;
     }
+
+    // caculate the coefficients of the quintuple polynomial
+    //      s(t) = a_0 + a_1*t + a_2*t^2 + a_3*t^3 + a_4*t^4 +a_5*t^5
+    //      constraints: s(0) = 0; sdot(0) = 0; sdotdot(0) = 0; s(T) = 1; sdot(T) = 0; sdotdot(T) = 0
     void _generateA345(){
         if(NearZero(_pathTime)){
             _a3 = 0;
@@ -107,6 +114,7 @@ int main(){
     forward << 0.0, 1.5, -1.0, -0.54, 0.0, 0.0;
     double speed = 0.5;
     arm.startTrack(ArmFSMState::JOINTCTRL);
+    // move from current posture to [forward]
     arm.jointTraj.setJointTraj(arm._ctrlComp->lowstate->getQ(), forward, speed);
     arm.jointTraj.setGripper(arm._ctrlComp->lowstate->getGripperQ(), -1, speed);
     
diff --git a/examples/example_lowCmd_send.cpp b/examples/example_lowCmd_send.cpp
index 17a7682..2a4b308 100644
--- a/examples/example_lowCmd_send.cpp
+++ b/examples/example_lowCmd_send.cpp
@@ -1,4 +1,4 @@
-#include "control/unitreeArm.h"
+#include "unitree_arm_sdk/control/unitreeArm.h"
 
 using namespace UNITREE_ARM;
 
diff --git a/include/unitree_arm_sdk/control/ctrlComponents.h b/include/unitree_arm_sdk/control/ctrlComponents.h
index b4faddc..5e1f273 100644
--- a/include/unitree_arm_sdk/control/ctrlComponents.h
+++ b/include/unitree_arm_sdk/control/ctrlComponents.h
@@ -1,12 +1,12 @@
 #ifndef CTRLCOMPONENTS_H
 #define CTRLCOMPONENTS_H
 
-#include "message/arm_common.h"
-#include "message/LowlevelCmd.h"
-#include "message/LowlevelState.h"
-#include "message/udp.h"
-#include "utilities/loop.h"
-#include "model/ArmModel.h"
+#include "unitree_arm_sdk/message/arm_common.h"
+#include "unitree_arm_sdk/message/LowlevelCmd.h"
+#include "unitree_arm_sdk/message/LowlevelState.h"
+#include "unitree_arm_sdk/message/udp.h"
+#include "unitree_arm_sdk/utilities/loop.h"
+#include "unitree_arm_sdk/model/ArmModel.h"
 
 namespace UNITREE_ARM {
 struct CtrlComponents{
diff --git a/include/unitree_arm_sdk/control/unitreeArm.h b/include/unitree_arm_sdk/control/unitreeArm.h
index 8fc4a2f..1687543 100644
--- a/include/unitree_arm_sdk/control/unitreeArm.h
+++ b/include/unitree_arm_sdk/control/unitreeArm.h
@@ -1,7 +1,7 @@
 #ifndef __UNITREEARM_H
 #define __UNITREEARM_H
 
-#include "control/ctrlComponents.h"
+#include "unitree_arm_sdk/control/ctrlComponents.h"
 
 namespace UNITREE_ARM {
 
@@ -77,7 +77,7 @@ bool MoveJ(Vec6 posture, double maxSpeed);
  * Input:    posture: target position, (roll pitch yaw x y z), unit: meter
  *           gripperPos: target angular
  *             uint: radian
- *             range:[0, pi/3]
+ *             range:[-pi/2, 0]
  *           maxSpeed: the maximum joint speed when robot is moving
  *             unit: radian/s
  *             range:[0, pi]
@@ -95,7 +95,7 @@ bool MoveL(Vec6 posture, double maxSpeed);
  * Function: Move the robot in a linear path, and control the gripper at the same time
  * Input:    posture: target position, (roll pitch yaw x y z), unit: meter
  *           gripperPos: target angular, uint: radian
- *             range:[0, pi/3]
+ *             range:[-pi/2, 0]
  *           maxSpeed: the maximum joint speed when robot is moving, unit: m/s
  * Output:   whether posture has inverse kinematics
  */
@@ -113,7 +113,7 @@ bool MoveC(Vec6 middlePosutre, Vec6 endPosture, double maxSpeed);
  * Input:    middle posture: determine the shape of the circular path
  *           endPosture: target position, (roll pitch yaw x y z), unit: meter
  *           gripperPos: target angular, uint: radian
- *             range:[0, pi/3]
+ *             range:[-pi/2, 0]
  *           maxSpeed: the maximum joint speed when robot is moving, unit: m/s
  * Output:   whether posture has inverse kinematics
  */
@@ -153,6 +153,24 @@ void startTrack(ArmFSMState fsm);
  */
 void sendRecv();
 
+/*
+ * Function: whether to wait for the command to finish
+ * Input:    true or false
+ * Output:   None
+ * Description: For example, MoveJ will send a trajectory command to z1_controller and then
+ *              run usleep() to wait for the trajectory execution to complete.
+ *              If set [wait] to false, MoveJ  will send command only and user should judge 
+ *              for youself whether the command is complete.
+ *                  Method 1: if(_ctrlComp->recvState.state != fsm)
+ *                      After trajectory complete, the FSM will switch to ArmFSMState::JOINTCTRL
+ *                      automatically
+ *                  Method 2: if((lowState->endPosture - endPostureGoal).norm() < error)
+ *                      Check whether current posture reaches the target
+ *              Related functions:
+ *                  MoveJ(), MoveL(), MoveC(), backToStart(), labelRun(), teachRepeat()
+ */
+void setWait(bool Y_N);
+
 //command parameters
 Vec6 q, qd, tau;
 Vec6 endPosture;
@@ -162,6 +180,9 @@ LoopFunc *sendRecvThread;
 LowlevelCmd *lowcmd;//same as _ctrlComp->lowcmd
 LowlevelState *lowstate;//same as _ctrlComp->lowstate
 CtrlComponents *_ctrlComp;
+
+private:
+    bool _isWait = true;
 };
 
 }
diff --git a/include/unitree_arm_sdk/math/mathTools.h b/include/unitree_arm_sdk/math/mathTools.h
index 420ab78..0b2b511 100644
--- a/include/unitree_arm_sdk/math/mathTools.h
+++ b/include/unitree_arm_sdk/math/mathTools.h
@@ -3,7 +3,7 @@
 
 #include <iostream>
 #include <math.h>
-#include "math/mathTypes.h"
+#include "unitree_arm_sdk/math/mathTypes.h"
 
 namespace UNITREE_ARM {
 
diff --git a/include/unitree_arm_sdk/message/LowlevelCmd.h b/include/unitree_arm_sdk/message/LowlevelCmd.h
index e63122d..e70c641 100644
--- a/include/unitree_arm_sdk/message/LowlevelCmd.h
+++ b/include/unitree_arm_sdk/message/LowlevelCmd.h
@@ -1,7 +1,7 @@
 #ifndef LOWLEVELCMD_H
 #define LOWLEVELCMD_H
 
-#include "math/mathTypes.h"
+#include "unitree_arm_sdk/math/mathTypes.h"
 #include <vector>
 #include <iostream>
 
diff --git a/include/unitree_arm_sdk/message/LowlevelState.h b/include/unitree_arm_sdk/message/LowlevelState.h
index 9386e9c..659f5b6 100644
--- a/include/unitree_arm_sdk/message/LowlevelState.h
+++ b/include/unitree_arm_sdk/message/LowlevelState.h
@@ -3,8 +3,8 @@
 
 #include <iostream>
 #include <vector>
-#include "math/mathTypes.h"
-#include "message/arm_common.h"
+#include "unitree_arm_sdk/math/mathTypes.h"
+#include "unitree_arm_sdk/message/arm_common.h"
 
 namespace UNITREE_ARM {
 
diff --git a/include/unitree_arm_sdk/message/arm_common.h b/include/unitree_arm_sdk/message/arm_common.h
index dcfe624..15f1a18 100644
--- a/include/unitree_arm_sdk/message/arm_common.h
+++ b/include/unitree_arm_sdk/message/arm_common.h
@@ -83,7 +83,7 @@ struct TrajCmd{
     double gripperPos;
     double maxSpeed;
     double stopTime;
-    int trajOrder;
+    int reserved;
 };
 
 union ValueUnion{
diff --git a/include/unitree_arm_sdk/message/udp.h b/include/unitree_arm_sdk/message/udp.h
index eaa9493..67086d6 100644
--- a/include/unitree_arm_sdk/message/udp.h
+++ b/include/unitree_arm_sdk/message/udp.h
@@ -6,7 +6,7 @@
 #include <string>
 #include <string.h>
 #include <vector>
-#include "utilities/timer.h"
+#include "unitree_arm_sdk/utilities/timer.h"
 
 namespace UNITREE_ARM {
 enum class BlockYN{
diff --git a/include/unitree_arm_sdk/model/ArmModel.h b/include/unitree_arm_sdk/model/ArmModel.h
index decfef2..9337bea 100644
--- a/include/unitree_arm_sdk/model/ArmModel.h
+++ b/include/unitree_arm_sdk/model/ArmModel.h
@@ -2,8 +2,8 @@
 #define ARMMODEL_H
 
 #include <vector>
-#include "thirdparty/robotics.h"
-#include "thirdparty/quadProgpp/QuadProg++.hh"
+#include "unitree_arm_sdk/thirdparty/robotics.h"
+#include "unitree_arm_sdk/thirdparty/quadProgpp/QuadProg++.hh"
 
 
 namespace UNITREE_ARM {
diff --git a/include/unitree_arm_sdk/thirdparty/quadProgpp/QuadProg++.hh b/include/unitree_arm_sdk/thirdparty/quadProgpp/QuadProg++.hh
index c2954d4..074dfb6 100644
--- a/include/unitree_arm_sdk/thirdparty/quadProgpp/QuadProg++.hh
+++ b/include/unitree_arm_sdk/thirdparty/quadProgpp/QuadProg++.hh
@@ -62,7 +62,7 @@ s.t.
 #ifndef _QUADPROGPP
 #define _QUADPROGPP
 
-#include "thirdparty/quadProgpp/Array.hh"
+#include "unitree_arm_sdk/thirdparty/quadProgpp/Array.hh"
 #include <eigen3/Eigen/Dense>
 
 namespace quadprogpp {
diff --git a/include/unitree_arm_sdk/thirdparty/robotics.h b/include/unitree_arm_sdk/thirdparty/robotics.h
index 3e96a54..a220435 100644
--- a/include/unitree_arm_sdk/thirdparty/robotics.h
+++ b/include/unitree_arm_sdk/thirdparty/robotics.h
@@ -1,7 +1,7 @@
 #pragma once
 
 #include <vector>
-#include "math/mathTools.h"
+#include "unitree_arm_sdk/math/mathTools.h"
 
 namespace robo {
 /*
diff --git a/include/unitree_arm_sdk/utilities/loop.h b/include/unitree_arm_sdk/utilities/loop.h
index 7db9e98..ecc8cdf 100644
--- a/include/unitree_arm_sdk/utilities/loop.h
+++ b/include/unitree_arm_sdk/utilities/loop.h
@@ -10,7 +10,7 @@
 #include <boost/shared_ptr.hpp>
 #include <boost/function.hpp>
 #include <boost/bind.hpp>
-#include "utilities/timer.h"
+#include "unitree_arm_sdk/utilities/timer.h"
 
 namespace UNITREE_ARM {
 typedef boost::function<void ()> Callback;
diff --git a/lib/libZ1_SDK_Linux64.so b/lib/libZ1_SDK_Linux64.so
index 33d82e6..a598235 100644
Binary files a/lib/libZ1_SDK_Linux64.so and b/lib/libZ1_SDK_Linux64.so differ