add three examples

CMakeLists.txt

@@ -21,3 +21,9 @@ add_executable(bigDemo examples/bigDemo.cpp )
 target_link_libraries(bigDemo Z1_SDK_Linux64)
 add_executable(example_lowCmd_send examples/example_lowCmd_send.cpp )
 target_link_libraries(example_lowCmd_send Z1_SDK_Linux64)
+add_executable(example_CtrlByFSM examples/example_CtrlByFSM.cpp )
+target_link_libraries(example_CtrlByFSM Z1_SDK_Linux64)
+add_executable(example_CtrlByTraj examples/example_CtrlByTraj.cpp )
+target_link_libraries(example_CtrlByTraj Z1_SDK_Linux64)
+add_executable(example_JointCtrl examples/example_JointCtrl.cpp )
+target_link_libraries(example_JointCtrl Z1_SDK_Linux64)

examples/example_CtrlByFSM.cpp

@@ -0,0 +1,37 @@
+#include "control/unitreeArm.h"
+
+int main() {
+    Vec6 posture[2];
+    CtrlComponents *ctrlComp = new CtrlComponents(0.002);
+    unitreeArm arm(ctrlComp);
+    arm.sendRecvThread->start();
+
+    arm.backToStart();
+    
+    //example
+    std::cout << "[JOINTCTRL]" << std::endl;
+    arm.setFsm(ArmFSMState::JOINTCTRL);
+
+    std::cout << "[TO STATE]" << std::endl;
+    arm.labelRun("forward");
+    
+    std::cout << "[MOVEJ]" << std::endl;
+    posture[0]<<0.5,0.1,0.1,0.5,-0.2,0.5;
+    arm.MoveJ(posture[0], -M_PI/3.0, 1.0);
+
+    std::cout << "[MOVEL]" << std::endl;
+    posture[0] << 0,0,0,0.45,-0.2,0.2;
+    arm.MoveL(posture[0], 0., 0.3);
+    
+    std::cout << "[MOVEC]" << std::endl;
+    posture[0] << 0,0,0,0.45,0,0.4;
+    posture[1] << 0,0,0,0.45,0.2,0.2;
+    arm.MoveC(posture[0], posture[1], -M_PI/3.0, 0.3);
+
+    //stop
+    arm.backToStart();
+    arm.setFsm(ArmFSMState::PASSIVE);
+    arm.sendRecvThread->shutdown();
+    delete ctrlComp;
+    return 0;
+}

examples/example_CtrlByTraj.cpp

@@ -0,0 +1,87 @@
+#include "control/unitreeArm.h"
+
+class Z1ARM : public unitreeArm{
+public:
+    Z1ARM(CtrlComponents * ctrlComp):unitreeArm(ctrlComp){};
+    ~Z1ARM(){};
+    void setJointTraj();
+    void setLineTraj();
+    void setCircleTraj();
+    void armCtrlTrackInCart();
+    void printState();
+};
+
+int main() {
+    CtrlComponents *ctrlComp = new CtrlComponents(0.002);
+    unitreeArm arm(ctrlComp);
+    arm.sendRecvThread->start();
+
+    arm.backToStart();
+
+    //example
+    Vec6 posture[2];
+    int order = 1;
+
+    arm.labelRun("forward");
+
+    arm._trajCmd.trajOrder = 0;//if order == 0, clear traj
+    arm.setTraj();
+
+    // No.1 trajectory
+    arm._trajCmd.trajOrder = order++;
+    arm._trajCmd.trajType = TrajType::MoveJ;
+    arm._trajCmd.maxSpeed = 1.0;// angular velocity, rad/s
+    arm._trajCmd.gripperPos = 0.0;
+    posture[0] << 0.5,0.1,0.1,0.5,-0.2,0.5;
+    arm._trajCmd.posture[0] = Vec6toPosture(posture[0]);
+    arm.setTraj();
+
+    // No.2 trajectory
+    arm._trajCmd.trajOrder = order++;
+    arm._trajCmd.trajType = TrajType::Stop;
+    arm._trajCmd.stopTime = 1.0;
+    arm._trajCmd.gripperPos = -1.0;
+    arm.setTraj();
+
+
+    // No.3 trajectory
+    arm._trajCmd.trajOrder = order++;
+    arm._trajCmd.trajType = TrajType::MoveL;
+    arm._trajCmd.maxSpeed = 0.3; // Cartesian velocity , m/s
+    arm._trajCmd.gripperPos = 0.0;
+    posture[0] << 0,0,0,0.45,-0.2,0.2;
+    arm._trajCmd.posture[0] = Vec6toPosture(posture[0]);
+    arm.setTraj();
+
+    // No.4 trajectory
+    arm._trajCmd.trajOrder = order++;
+    arm._trajCmd.trajType = TrajType::Stop;
+    arm._trajCmd.stopTime = 1.0; 
+    arm._trajCmd.gripperPos = -1.0;
+    arm.setTraj();
+
+    // No.5 trajectory
+    arm._trajCmd.trajOrder = order++;
+    arm._trajCmd.trajType = TrajType::MoveC;
+    arm._trajCmd.maxSpeed = 0.3; // Cartesian velocity
+    arm._trajCmd.gripperPos = 0.0;
+    posture[0] << 0,0,0,0.45,0,0.4;
+    posture[1] << 0,0,0,0.45,0.2,0.2;
+    arm._trajCmd.posture[0] = Vec6toPosture(posture[0]);
+    arm._trajCmd.posture[1] = Vec6toPosture(posture[1]);
+    arm.setTraj();
+
+    arm.startTraj();
+    // wait for trajectory completion
+    while (arm._ctrlComp->recvState.state != ArmFSMState::JOINTCTRL){
+        usleep(arm._ctrlComp->dt*1000000);
+    }
+
+
+    //stop
+    arm.backToStart();
+    arm.setFsm(ArmFSMState::PASSIVE);
+    arm.sendRecvThread->shutdown();
+    delete ctrlComp;
+    return 0;
+}

examples/example_JointCtrl.cpp

@@ -0,0 +1,124 @@
+#include "control/unitreeArm.h"
+
+class JointTraj{
+public:
+    JointTraj(){};
+    ~JointTraj(){};
+    void setJointTraj(Vec6 startQ, Vec6 endQ, double speed){
+        _startQ = startQ;
+        _endQ = endQ;
+        Vec6 deltaQ = endQ - startQ;
+        Vec6 t = deltaQ / speed;
+        for(int i(0); i<6; i++){
+            _pathTime = fabs(t(i)) > _pathTime ? fabs(t(i)) : _pathTime;
+        }
+        _generateA345();
+    };
+    void setGripper(double startQ, double endQ, double speed){
+        _gripperStartQ = startQ;
+        _gripperEndQ = endQ; 
+        double t = fabs(endQ - startQ)/speed;
+        _pathTime = t > _pathTime ? t : _pathTime;
+        _generateA345();
+    }
+    bool getJointCmd(Vec6& q, Vec6& qd, double& gripperQ, double& gripperQd){
+        _runTime();
+
+        if(!_reached){
+            _s = _a3*pow(_tCost,3) + _a4*pow(_tCost,4) + _a5*pow(_tCost,5);
+            _sDot = 3*_a3*pow(_tCost,2) + 4*_a4*pow(_tCost,3) + 5*_a5*pow(_tCost,4);
+            q = _startQ + _s*(_endQ - _startQ);
+            qd = _sDot * (_endQ - _startQ);
+
+            gripperQ  = _gripperStartQ + (_gripperEndQ-_gripperStartQ)*_s;
+            gripperQd = (_gripperEndQ-_gripperStartQ) * _sDot;
+        }else{
+            q = _endQ;
+            qd.setZero();
+        }
+
+        return _reached;
+    }
+private:
+    //joint trajectory parameters
+    Vec6 _startQ, _endQ;
+    double _gripperStartQ, _gripperEndQ;
+    double _pathTime, _tCost, _currentTime, _startTime;
+    double _a3, _a4, _a5;
+    double _s, _sDot;       // [0, 1]
+    bool _pathStarted = false;
+    bool _reached = false;
+
+    void _runTime(){
+        _currentTime = getTimeSecond();
+
+        if(!_pathStarted){
+            _pathStarted = true;
+            _startTime = _currentTime;
+            _tCost = 0;
+        }
+        _tCost = _currentTime - _startTime;
+        _reached = (_tCost>_pathTime) ? true : false;
+        _tCost = (_tCost>_pathTime) ? _pathTime : _tCost;
+    }
+    void _generateA345(){
+        if(_pathTime == 0){
+            _a3 = 0;
+            _a4 = 0;
+            _a5 = 0;
+        }else{
+            _a3 =  10 / pow(_pathTime, 3);   // parameters of path
+            _a4 = -15 / pow(_pathTime, 4);
+            _a5 =   6 / pow(_pathTime, 5);
+        }
+    }
+};
+
+
+class Z1ARM : public unitreeArm{
+public:
+    Z1ARM(CtrlComponents * ctrlComp):unitreeArm(ctrlComp){
+        myThread = new LoopFunc("Z1Communication", _ctrlComp->dt, boost::bind(&Z1ARM::run, this));
+    };
+    ~Z1ARM(){delete myThread;};
+    void setJointTraj(Vec6 startQ,Vec6 endQ, double speed){
+    };
+    void run(){
+        if(trajStart){
+            if(jointTraj.getJointCmd(q, qd, gripperQ, gripperW)){
+                trajStart = false;
+            }
+        }
+        // std::cout << q.transpose() <<" "<< gripperQ <<std::endl;
+        sendRecv();
+    }
+
+    JointTraj jointTraj;
+    bool trajStart = false;
+    LoopFunc *myThread;
+};
+
+
+int main(){
+    std::cout << std::fixed << std::setprecision(3);
+    CtrlComponents *ctrlComp = new CtrlComponents(0.002);
+    Z1ARM arm(ctrlComp);
+    arm.myThread->start();
+
+    Vec6 forward;
+    forward << 0.0, 1.5, -1.0, -0.54, 0.0, 0.0;
+    double speed = 0.5;
+    arm.startTrack(ArmFSMState::JOINTCTRL);
+    arm.jointTraj.setJointTraj(arm._ctrlComp->lowstate->getQ(), forward, speed);
+    arm.jointTraj.setGripper(arm._ctrlComp->lowstate->getGripperQ(), -1, speed);
+    
+    arm.trajStart = true;
+    while (arm.trajStart){
+        sleep(1);
+    }
+    arm.backToStart();
+    arm.setFsm(ArmFSMState::PASSIVE);
+    arm.myThread->shutdown();
+    delete ctrlComp;
+    return 0;
+}

src/control/unitreeArm.cpp

@@ -27,6 +27,7 @@ void unitreeArm::setFsm(ArmFSMState fsm){
 
 void unitreeArm::backToStart(){
     setFsm(ArmFSMState::BACKTOSTART);
+    _ctrlComp->sendCmd.track = false;
     while (_ctrlComp->recvState.state != ArmFSMState::JOINTCTRL){
         usleep(_ctrlComp->dt * 1000000);
     }