fix cartesian path bug

Author: Agnel-Wang

.gitignore

@@ -0,0 +1,2 @@
+.vscode
+build

CMakeLists.txt

@@ -5,7 +5,7 @@ set(PACKAGE_NAME z1_controller)
 set(CMAKE_CXX_FLAGS "$ENV{CXXFLAGS} -O3")
 
 # ----------------------options----------------------
-set(COMMUNICATION UDP)            # UDP
+ set(COMMUNICATION UDP)            # UDP
 # set(COMMUNICATION ROS)          # ROS
 
 

include/FSM/BaseState.h

@@ -1,32 +1,32 @@
-#ifndef BASESTATE_H
-#define BASESTATE_H
-
-#include <string>
-#include "common/enumClass.h"
-
-class BaseState{
-public:
-    BaseState(int stateNameEnum, std::string stateNameString)
-        : _stateNameEnum(stateNameEnum), _stateNameString(stateNameString){}
-    virtual ~BaseState(){};
-
-    virtual void enter() = 0;
-    virtual void run() = 0;
-    virtual void exit() = 0;
-    virtual int checkChange(int cmd) = 0;
-
-    bool isState(int stateEnum){
-        if(_stateNameEnum == stateEnum){
-            return true;
-        }else{
-            return false;
-        }
-    }
-    std::string getStateName(){return _stateNameString;}
-    int getStateNameEnum(){return _stateNameEnum;};
-protected:
-    int _stateNameEnum;
-    std::string _stateNameString;
-};
-
+#ifndef BASESTATE_H
+#define BASESTATE_H
+
+#include <string>
+#include "common/enumClass.h"
+
+class BaseState{
+public:
+    BaseState(int stateNameEnum, std::string stateNameString)
+        : _stateNameEnum(stateNameEnum), _stateNameString(stateNameString){}
+    virtual ~BaseState(){};
+
+    virtual void enter() = 0;
+    virtual void run() = 0;
+    virtual void exit() = 0;
+    virtual int checkChange(int cmd) = 0;
+
+    bool isState(int stateEnum){
+        if(_stateNameEnum == stateEnum){
+            return true;
+        }else{
+            return false;
+        }
+    }
+    std::string getStateName(){return _stateNameString;}
+    int getStateNameEnum(){return _stateNameEnum;};
+protected:
+    int _stateNameEnum;
+    std::string _stateNameString;
+};
+
 #endif

include/FSM/FSMState.h

@@ -1,49 +1,49 @@
-#ifndef FSMSTATE_H
-#define FSMSTATE_H
-
-#include <string>
-#include <iostream>
-#include <unistd.h>
-#include "control/CtrlComponents.h"
-#include "common/math/mathTools.h"
-#include "common/utilities/timer.h"
-#include "FSM/BaseState.h"
-
-class FSMState : public BaseState{
-public:
-    FSMState(CtrlComponents *ctrlComp, ArmFSMStateName stateName, std::string stateNameString);
-    virtual ~FSMState(){}
-
-    virtual void enter() = 0;
-    virtual void run() = 0;
-    virtual void exit() = 0;
-    virtual int checkChange(int cmd) {return (int)ArmFSMStateName::INVALID;}
-    bool _collisionTest();
-    
-protected:
-    void _armCtrl();
-    void _recordData();
-    Vec6 _postureToVec6(Posture posture);
-    void _tauFriction();
-
-    LowlevelCmd *_lowCmd;
-    LowlevelState *_lowState;
-    IOInterface *_ioInter;
-    ArmModel *_armModel;
-
-    Vec6 _qPast, _qdPast, _q, _qd, _qdd, _tauForward;
-    double _gripperPos, _gripperW, _gripperTau;
-
-    CtrlComponents *_ctrlComp;
-    Vec6 _g, _tauCmd, _tauFric;
-
-private:
-
-    uint _collisionCnt;
-
-    Vec6 _mLinearFriction;
-    Vec6 _mCoulombFriction;
-
-};
-
-#endif  // FSMSTATE_H
+#ifndef FSMSTATE_H
+#define FSMSTATE_H
+
+#include <string>
+#include <iostream>
+#include <unistd.h>
+#include "control/CtrlComponents.h"
+#include "common/math/mathTools.h"
+#include "common/utilities/timer.h"
+#include "FSM/BaseState.h"
+
+class FSMState : public BaseState{
+public:
+    FSMState(CtrlComponents *ctrlComp, ArmFSMStateName stateName, std::string stateNameString);
+    virtual ~FSMState(){}
+
+    virtual void enter() = 0;
+    virtual void run() = 0;
+    virtual void exit() = 0;
+    virtual int checkChange(int cmd) {return (int)ArmFSMStateName::INVALID;}
+    bool _collisionTest();
+    
+protected:
+    void _armCtrl();
+    void _recordData();
+    Vec6 _postureToVec6(Posture posture);
+    void _tauFriction();
+
+    LowlevelCmd *_lowCmd;
+    LowlevelState *_lowState;
+    IOInterface *_ioInter;
+    ArmModel *_armModel;
+
+    Vec6 _qPast, _qdPast, _q, _qd, _qdd, _tauForward;
+    double _gripperPos, _gripperW, _gripperTau;
+
+    CtrlComponents *_ctrlComp;
+    Vec6 _g, _tauCmd, _tauFric;
+
+private:
+
+    uint _collisionCnt;
+
+    Vec6 _mLinearFriction;
+    Vec6 _mCoulombFriction;
+
+};
+
+#endif  // FSMSTATE_H

include/FSM/FiniteStateMachine.h

@@ -1,28 +1,28 @@
-#ifndef FSM_H
-#define FSM_H
-
-#include <vector>
-#include "FSM/FSMState.h"
-#include "common/utilities/loop.h"
-#include "control/CtrlComponents.h"
-
-class FiniteStateMachine{
-public:
-    FiniteStateMachine(std::vector<FSMState*> states, CtrlComponents *ctrlComp);
-    virtual ~FiniteStateMachine();
-
-private:
-    void _run();
-    std::vector<FSMState*> _states;
-
-    FSMMode _mode;
-    bool _running;
-    FSMState* _currentState;
-    FSMState* _nextState;
-    int _nextStateEnum;
-
-    CtrlComponents *_ctrlComp;
-    LoopFunc *_runThread;
-};
-
+#ifndef FSM_H
+#define FSM_H
+
+#include <vector>
+#include "FSM/FSMState.h"
+#include "common/utilities/loop.h"
+#include "control/CtrlComponents.h"
+
+class FiniteStateMachine{
+public:
+    FiniteStateMachine(std::vector<FSMState*> states, CtrlComponents *ctrlComp);
+    virtual ~FiniteStateMachine();
+
+private:
+    void _run();
+    std::vector<FSMState*> _states;
+
+    FSMMode _mode;
+    bool _running;
+    FSMState* _currentState;
+    FSMState* _nextState;
+    int _nextStateEnum;
+
+    CtrlComponents *_ctrlComp;
+    LoopFunc *_runThread;
+};
+
 #endif  // FSM_H

include/FSM/State_BackToStart.h

@@ -1,22 +1,22 @@
-#ifndef STATE_BACKTOSTART_H
-#define STATE_BACKTOSTART_H
-
-
-#include "FSM/FSMState.h"
-#include "trajectory/JointSpaceTraj.h"
-
-class State_BackToStart : public FSMState{
-public:
-    State_BackToStart(CtrlComponents *ctrlComp);
-    ~State_BackToStart();
-    void enter();
-    void run();
-    void exit();
-    int checkChange(int cmd);
-private:
-    bool _reach, _pastReach;
-    JointSpaceTraj *_jointTraj;
-    Vec6 _pos_startFlat;
-};
-
+#ifndef STATE_BACKTOSTART_H
+#define STATE_BACKTOSTART_H
+
+
+#include "FSM/FSMState.h"
+#include "trajectory/JointSpaceTraj.h"
+
+class State_BackToStart : public FSMState{
+public:
+    State_BackToStart(CtrlComponents *ctrlComp);
+    ~State_BackToStart();
+    void enter();
+    void run();
+    void exit();
+    int checkChange(int cmd);
+private:
+    bool _reach, _pastReach;
+    JointSpaceTraj *_jointTraj;
+    Vec6 _pos_startFlat;
+};
+
 #endif  // STATE_BACKTOSTART_H

include/FSM/State_Calibration.h

@@ -1,18 +1,18 @@
-#ifndef STATE_CALIBRATION_H
-#define STATE_CALIBRATION_H
-
-#include "FSM/FSMState.h"
-
-class State_Calibration : public FSMState{
-public:
-    State_Calibration(CtrlComponents *ctrlComp);
-    ~State_Calibration(){}
-    void enter();
-    void run(){};
-    void exit(){};
-    int checkChange(int cmd);
-private:
-
-};
-
+#ifndef STATE_CALIBRATION_H
+#define STATE_CALIBRATION_H
+
+#include "FSM/FSMState.h"
+
+class State_Calibration : public FSMState{
+public:
+    State_Calibration(CtrlComponents *ctrlComp);
+    ~State_Calibration(){}
+    void enter();
+    void run(){};
+    void exit(){};
+    int checkChange(int cmd);
+private:
+
+};
+
 #endif  // STATE_CALIBRATION_H

include/FSM/State_Cartesian.h

@@ -16,7 +16,7 @@ class State_Cartesian : public FSMState{
     double _posSpeed = 0.2;
     double oriSpeedLimit = 0.5;// limits in SDK
     double posSpeedLimit = 0.5;
-    VecX _changeDirections;
+    VecX _changeDirectionsf;
 
     Vec6 _twist;
     HomoMat _endHomoGoal, _endHomoPast, _endHomoDelta;

include/FSM/State_JointSpace.h

@@ -1,18 +1,18 @@
-#ifndef JOINTSPACE_H
-#define JOINTSPACE_H
-
-#include "FSM/FSMState.h"
-
-class State_JointSpace : public FSMState{
-public:
-    State_JointSpace(CtrlComponents *ctrlComp);
-    ~State_JointSpace(){}
-    void enter();
-    void run();
-    void exit();
-    int checkChange(int cmd);
-private:
-    std::vector<double> jointSpeedMax;
-};
-
+#ifndef JOINTSPACE_H
+#define JOINTSPACE_H
+
+#include "FSM/FSMState.h"
+
+class State_JointSpace : public FSMState{
+public:
+    State_JointSpace(CtrlComponents *ctrlComp);
+    ~State_JointSpace(){}
+    void enter();
+    void run();
+    void exit();
+    int checkChange(int cmd);
+private:
+    std::vector<double> jointSpeedMax;
+};
+
 #endif  // JOINTSPACE_H

include/FSM/State_LowCmd.h

@@ -1,18 +1,18 @@
-#ifndef LOWCMD_H
-#define LOWCMD_H
-
-#include "FSMState.h"
-
-class State_LowCmd : public FSMState{
-public:
-    State_LowCmd(CtrlComponents *ctrlComp);
-    void enter();
-    void run();
-    void exit();
-    int checkChange(int cmd);
-private:
-    std::vector<float> _kp;
-    std::vector<float> _kw;
-};
-
+#ifndef LOWCMD_H
+#define LOWCMD_H
+
+#include "FSMState.h"
+
+class State_LowCmd : public FSMState{
+public:
+    State_LowCmd(CtrlComponents *ctrlComp);
+    void enter();
+    void run();
+    void exit();
+    int checkChange(int cmd);
+private:
+    std::vector<float> _kp;
+    std::vector<float> _kw;
+};
+
 #endif  // LOWCMD_H