Reduced acceleration

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

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