diff --git a/include/FSM/FSMState.h b/include/FSM/FSMState.h
index a983cc8..e505e7b 100755
--- a/include/FSM/FSMState.h
+++ b/include/FSM/FSMState.h
@@ -30,11 +30,21 @@ protected:
     IOInterface *_ioInter;
     ArmModel *_armModel;
 
-    Vec6 _qPast, _qdPast, _q, _qd, _qdd, _tauf, _tauCmd, _g;
+    Vec6 _qPast, _qdPast, _q, _qd, _qdd, _tauForward;
     double _gripperPos, _gripperW, _gripperTau;
-    uint _collisionCnt;
 
     CtrlComponents *_ctrlComp;
+    Vec6 _g, _tauCmd;
+
+private:
+    void _tauFriction();
+
+    uint _collisionCnt;
+
+    Vec6 _mLinearFriction;
+    Vec6 _mCoulombFriction;
+    Vec6 _tauFric;
+
 };
 
 #endif  // FSMSTATE_H
diff --git a/include/trajectory/JointSpaceTraj.h b/include/trajectory/JointSpaceTraj.h
index a63cce4..bf374d1 100755
--- a/include/trajectory/JointSpaceTraj.h
+++ b/include/trajectory/JointSpaceTraj.h
@@ -7,6 +7,7 @@
 #include "control/CtrlComponents.h"
 #include "trajectory/SCurve.h"
 
+
 class JointSpaceTraj : public Trajectory{
 public:
     JointSpaceTraj(CtrlComponents *ctrlComp);
@@ -20,9 +21,13 @@ public:
     bool setJointTraj(Vec6 startQ, std::string endName, double speed);
     bool setJointTraj(std::string startName, std::string endName, double speed);
 private:
+    void _generateA345(double pathTime);
+
     SCurve _jointCurve;
     double ddQMax;
     double dddQMax;
+
+    double _a3, _a4, _a5, _s, _sDot;
 };
 
 #endif  // JOINTSPACETRAJ_H
\ No newline at end of file
diff --git a/lib/libZ1_ROS_Linux64.so b/lib/libZ1_ROS_Linux64.so
index 21adce2..df90e20 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 6a78c90..69dc1b5 100644
Binary files a/lib/libZ1_UDP_Linux64.so and b/lib/libZ1_UDP_Linux64.so differ