V2.2.4

1. add keyboard speed filter
2. fix ros gripper bug: abnormal jitter

CMakeLists.txt

@@ -52,15 +52,16 @@ if(SIMULATION)
 endif()
 
 set(EIGEN_PATH /usr/include/eigen3)
+find_package(Boost REQUIRED)
 include_directories(
     include
-    ${Boost_INCLUDE_DIR}
+    ${Boost_INCLUDE_DIRS}
     ${EIGEN_PATH}
 )
 
 link_directories(lib)
 
 # ----------------------add executable----------------------
-set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/../build)                # EXECUTABLE:CMAKE_RUNTIME_OUTPUT_DIRECTORY
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR})
 add_executable(z1_ctrl main.cpp)
 target_link_libraries(z1_ctrl libZ1_${COMMUNICATION}_Linux64.so ${catkin_LIBRARIES} )

config/savedArmStates.csv

@@ -1,5 +1,5 @@
 forward, 0.0, 1.5, -1.0, -0.54, 0.0, 0.0, 
 startFlat, 0.0, 0.0, -0.005, -0.074, 0.0, 0.0, 
 show_left, -1.0, 0.9, -1., -0.3, 0.0, 0.0, 
-show_mid, 0.0, 0.9, -1.2, 0.3, 0.0, 0.0, 
+show_mid, 0.0, 0.9, -1.2, 0.25, 0.0, 0.0, 
 show_right, 1.0, 0.9, -1., -0.3, 0.0, 0.0, 

include/FSM/State_SetTraj.h

@@ -1,20 +0,0 @@
-#ifndef STATE_SETTRAJ_H
-#define STATE_SETTRAJ_H
-
-#include "FSM/State_Trajectory.h"
-
-class State_SetTraj: public State_Trajectory{
-public:
-    State_SetTraj(CtrlComponents *ctrlComp, 
-        ArmFSMStateName stateEnum = ArmFSMStateName::SETTRAJ, 
-        std::string stateString = "setTraj");
-    ~State_SetTraj();
-    void enter();
-    void run();
-    void exit();
-    int checkChange(int cmd);
-private:
-    Vec6 _posture[2];
-};
-
-#endif

include/FSM/State_Trajectory.h

@@ -16,7 +16,7 @@ public:
     int checkChange(int cmd);
 protected:
     void _setTraj();
-    void _setTrajSDK();
+    
     TrajectoryManager *_traj;
     HomoMat _goalHomo;
     Vec6 _goalTwist;

include/common/enumClass.h

@@ -20,7 +20,7 @@ enum class ArmFSMStateName{
     TEACH,
     TEACHREPEAT,
     CALIBRATION,
-    SETTRAJ,
+    SETTRAJ,//no longer used
     BACKTOSTART,
     NEXT,
     LOWCMD

include/control/cmdPanel.h

@@ -72,10 +72,11 @@ public:
     void setValue(double value){_value = value;}
 private:
     double _value;
-    double _changeDirection;
+    double _changeDirection=0.0;
     double _dV = 0.0; //delta value per delta time
     double _dt = 0.0;
     double _dVdt = 0.0; // delta value per second
+    double _dVdtf = 0.0; // delta value per second after filter
     double _lim1, _lim2;
     bool _hasLim = false;
     bool _hasGoZero = false;

include/message/arm_common.h

@@ -20,7 +20,7 @@ enum class ArmFSMState{
     TEACH,
     TEACHREPEAT,
     CALIBRATION,
-    SETTRAJ,
+    SETTRAJ,//no longer used
     BACKTOSTART,
     NEXT,
     LOWCMD

include/model/ArmModel.h

@@ -26,10 +26,10 @@ public:
     std::vector<double> getJointQMin() {return _jointQMin;}
     std::vector<double> getJointSpeedMax() {return _jointSpeedMax;}
     void addLoad(double load);
+    bool checkAngle(Vec6 );
 
     const size_t dof = 6;
 protected:
-    bool _checkAngle(Vec6 );
     void _buildModel();
     // initial parameters
     HomoMat _M; //End posture at the home position

main.cpp

@@ -19,12 +19,6 @@
 
 bool running = true;
 
-// over watch the ctrl+c command
-void ShutDown(int sig){
-    running = false;
-	std::cout << "[STATE] stop the controller" << std::endl;
-}
-
 //set real-time program
 void setProcessScheduler(){
     pid_t pid = getpid();
@@ -96,13 +90,12 @@ int main(int argc, char **argv){
     states.push_back(new State_ToState(ctrlComp));
     states.push_back(new State_Trajectory(ctrlComp));
     states.push_back(new State_Calibration(ctrlComp));
-    // states.push_back(new State_SetTraj(ctrlComp));
 
     FiniteStateMachine *fsm;
     fsm = new FiniteStateMachine(states, ctrlComp);
 
     ctrlComp->running = &running;
-    signal(SIGINT, ShutDown);
+    signal(SIGINT, [](int signum){running = false;});
     while(running){
         usleep(100000);
     }