increase joint acceleration

version 2.2.4

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;
 
 };
 

include/FSM/State_BackToStart.h

@@ -16,6 +16,7 @@ public:
 private:
     bool _reach, _pastReach;
     JointSpaceTraj *_jointTraj;
+    Vec6 _pos_startFlat;
 };
 
 #endif  // STATE_BACKTOSTART_H

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;

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;

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

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

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;
 };