diff --git a/include/FSM/FSMState.h b/include/FSM/FSMState.h
index e505e7b..3fafdeb 100755
--- a/include/FSM/FSMState.h
+++ b/include/FSM/FSMState.h
@@ -24,6 +24,7 @@ protected:
     void _armCtrl();
     void _recordData();
     Vec6 _postureToVec6(Posture posture);
+    void _tauFriction();
 
     LowlevelCmd *_lowCmd;
     LowlevelState *_lowState;
@@ -34,16 +35,14 @@ protected:
     double _gripperPos, _gripperW, _gripperTau;
 
     CtrlComponents *_ctrlComp;
-    Vec6 _g, _tauCmd;
+    Vec6 _g, _tauCmd, _tauFric;
 
 private:
-    void _tauFriction();
 
     uint _collisionCnt;
 
     Vec6 _mLinearFriction;
     Vec6 _mCoulombFriction;
-    Vec6 _tauFric;
 
 };
 
diff --git a/include/FSM/State_BackToStart.h b/include/FSM/State_BackToStart.h
index 9399e13..61f239f 100644
--- a/include/FSM/State_BackToStart.h
+++ b/include/FSM/State_BackToStart.h
@@ -16,6 +16,7 @@ public:
 private:
     bool _reach, _pastReach;
     JointSpaceTraj *_jointTraj;
+    Vec6 _pos_startFlat;
 };
 
 #endif  // STATE_BACKTOSTART_H
\ No newline at end of file
diff --git a/include/FSM/State_Cartesian.h b/include/FSM/State_Cartesian.h
index f12ba13..0b5ada3 100755
--- a/include/FSM/State_Cartesian.h
+++ b/include/FSM/State_Cartesian.h
@@ -12,8 +12,8 @@ public:
     void exit();
     int checkChange(int cmd);
 private:
-    double _oriSpeed = 0.4;// control by keyboard
-    double _posSpeed = 0.3;
+    double _oriSpeed = 0.3;// control by keyboard
+    double _posSpeed = 0.2;
     double oriSpeedLimit = 0.5;// limits in SDK
     double posSpeedLimit = 0.5;
     VecX _changeDirections;
diff --git a/include/control/CtrlComponents.h b/include/control/CtrlComponents.h
index 89f9762..6b4e242 100755
--- a/include/control/CtrlComponents.h
+++ b/include/control/CtrlComponents.h
@@ -23,7 +23,7 @@ public:
     std::string armConfigPath;
     CmdPanel *cmdPanel;
     IOInterface *ioInter;
-    ArmModel *armModel;
+    Z1Model *armModel;
     CSVTool *stateCSV;
 
     SendCmd sendCmd; // cmd that receive from SDK 
@@ -43,8 +43,7 @@ public:
     void geneObj();
     void writeData();
 private:
-    void inputProcess(int argc, char** argv);
-    void configProcess();
+    void configProcess(int argc, char** argv);
 
     std::string ctrl_IP;
     uint ctrl_port;
diff --git a/include/message/LowlevelState.h b/include/message/LowlevelState.h
index 9dee0ff..5a60dfd 100755
--- a/include/message/LowlevelState.h
+++ b/include/message/LowlevelState.h
@@ -55,6 +55,7 @@ private:
     size_t _dof = 6;
     int temporatureLimit = 80.0;// centigrade
     std::vector<int> _isMotorConnectedCnt;
+    std::vector<bool> _isMotorLostConnection;
 };
 
 #endif  //LOWLEVELSTATE_HPP
diff --git a/include/model/ArmModel.h b/include/model/ArmModel.h
index acc5353..c4a1099 100644
--- a/include/model/ArmModel.h
+++ b/include/model/ArmModel.h
@@ -66,11 +66,11 @@ public:
     bool checkInSingularity(Vec6 q);
     bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false);
     void solveQP(Vec6 twist, Vec6 qPast, Vec6& qd_result, double dt);
+    double _theta2Bias;
 private:
     void setParam_V3_6();
-    void setParam_V3_7();//long
+    void setParam_V3_7();
     void setParam_V3_8();
-    double _theta2Bias;
 };
 
 #endif
\ No newline at end of file
diff --git a/include/trajectory/JointSpaceTraj.h b/include/trajectory/JointSpaceTraj.h
index bf374d1..8533c06 100755
--- a/include/trajectory/JointSpaceTraj.h
+++ b/include/trajectory/JointSpaceTraj.h
@@ -24,8 +24,8 @@ private:
     void _generateA345(double pathTime);
 
     SCurve _jointCurve;
-    double ddQMax;
-    double dddQMax;
+    const double ddQMax = 15.0;
+    const double dddQMax = 30.0;
 
     double _a3, _a4, _a5, _s, _sDot;
 };
diff --git a/lib/libZ1_ROS_Linux64.so b/lib/libZ1_ROS_Linux64.so
index df90e20..7147f12 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 69dc1b5..feecccb 100644
Binary files a/lib/libZ1_UDP_Linux64.so and b/lib/libZ1_UDP_Linux64.so differ