diff --git a/CMakeLists.txt b/CMakeLists.txt
index a9f4b81..192c216 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -84,6 +84,7 @@ link_directories(lib)
 # ----------------------add executable----------------------
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/../build)                # EXECUTABLE:CMAKE_RUNTIME_OUTPUT_DIRECTORY
 add_executable(z1_ctrl main.cpp)
+# add_dependencies(z1_ctrl  Z1_${COMMUNICATION}_Linux64)
 target_link_libraries(z1_ctrl ${catkin_LIBRARIES} libUnitree_motor_SDK_Linux64_EPOLL.so libZ1_${COMMUNICATION}_Linux64.so)
 
 # set(EXECUTABLE_OUTPUT_PATH /home/$ENV{USER}/)
diff --git a/config.xml b/config.xml
index 5208b07..6547186 100644
--- a/config.xml
+++ b/config.xml
@@ -1,5 +1,5 @@
 <configurartion>
-    <control set="1">
+    <control set="2">
         <option1>keyboard</option1>
         <option2>sdk</option2>
         <option3>joystick</option3> <!-- default aliengo_joystick -->
diff --git a/include/FSM/State_TeachRepeat.h b/include/FSM/State_TeachRepeat.h
index 4961889..aad6bbf 100755
--- a/include/FSM/State_TeachRepeat.h
+++ b/include/FSM/State_TeachRepeat.h
@@ -21,6 +21,7 @@ private:
     size_t _index = 0;
     size_t _indexPast;
     Vec6 _trajStartQ, _trajStartQd;
+    double _trajStartGripperQ, _trajStartGripperQd;
 
     CSVTool *_csvFile;
 };
diff --git a/include/interface/IOUDP.h b/include/interface/IOUDP.h
index 6ba770e..2697889 100644
--- a/include/interface/IOUDP.h
+++ b/include/interface/IOUDP.h
@@ -2,7 +2,7 @@
 #define IOUDP_H
 
 #include "interface/IOInterface.h"
-#include "/usr/include/x86_64-linux-gnu/bits/floatn-common.h"
+// #include "/usr/include/x86_64-linux-gnu/bits/floatn-common.h"
 
 class IOUDP : public IOInterface{
 public:
diff --git a/include/trajectory/JointSpaceTraj.h b/include/trajectory/JointSpaceTraj.h
index 48c8d46..3fc84e5 100755
--- a/include/trajectory/JointSpaceTraj.h
+++ b/include/trajectory/JointSpaceTraj.h
@@ -31,6 +31,7 @@ private:
     bool _checkJointAngleValid(const double &q, int jointOrder);
     void _generateA345();
 
+    SCurve _jointCurve;
     int _jointNum;
     double _pathTimeTemp;       // path total time
     double _s, _sDot;       // [0, 1]
@@ -38,6 +39,7 @@ private:
 
     int _trajOrder;     // The order of trajectory curve
     std::vector<Vec6> _curveParam;
+
 };
 
 #endif  // JOINTSPACETRAJ_H
\ No newline at end of file
diff --git a/lib/libZ1_ROS_Linux64.so b/lib/libZ1_ROS_Linux64.so
index 79bf905..bffbfc6 100644
Binary files a/lib/libZ1_ROS_Linux64.so and b/lib/libZ1_ROS_Linux64.so differ
diff --git a/lib/libZ1_UDP_Linux64.so b/lib/libZ1_UDP_Linux64.so
index 85f9b2a..0ab20e8 100644
Binary files a/lib/libZ1_UDP_Linux64.so and b/lib/libZ1_UDP_Linux64.so differ
diff --git a/main.cpp b/main.cpp
index 40e1720..4138b19 100755
--- a/main.cpp
+++ b/main.cpp
@@ -110,7 +110,7 @@ int main(int argc, char **argv){
         
         cmdPanel = new Keyboard(events, emptyAction);
     }else if(ctrlComp->ctrl == Control::_JOYSTICK){
-        events.push_back(new StateAction("r12", (int)ArmFSMStateName::PASSIVE));
+        events.push_back(new StateAction("r2x", (int)ArmFSMStateName::TRAJECTORY));
         events.push_back(new StateAction("l12", (int)ArmFSMStateName::PASSIVE));
         events.push_back(new StateAction("r2", (int)ArmFSMStateName::JOINTCTRL));
         events.push_back(new StateAction("r1", (int)ArmFSMStateName::CARTESIAN));