fix lowcmd bug

CMakeLists.txt

@@ -22,6 +22,7 @@ foreach(EXAMPLE_FILE IN LISTS EXAMPLES_FILES)
 endforeach()
 
 
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
 find_package(pybind11 QUIET)
 if(${pybind11_FOUND})
   pybind11_add_module(unitree_arm_interface examples_py/arm_python_interface.cpp)

examples/highcmd_basic.cpp

@@ -44,11 +44,12 @@ void Z1ARM::armCtrlInJointCtrl(){
     labelRun("forward");
     startTrack(ArmFSMState::JOINTCTRL);
 
+    Timer timer(_ctrlComp->dt);
     for(int i(0); i<1000;i++){
         directions<< 0, 0, 0, -1, 0, 0, -1;
         joint_speed = 1.0;
         jointCtrlCmd(directions, joint_speed);
-        usleep(_ctrlComp->dt*1000000);
+        timer.sleep();
     }
 }
 
@@ -58,11 +59,12 @@ void Z1ARM::armCtrlInCartesian(){
     
     double angular_vel = 0.3;
     double linear_vel = 0.3;
+    Timer timer(_ctrlComp->dt);
     for(int i(0); i<2000;i++){
         directions<< 0, 0, 0, 0, 0, -1, -1;
 
         cartesianCtrlCmd(directions, angular_vel, linear_vel);
-        usleep(_ctrlComp->dt*1000000);
+        timer.sleep();
     }
 }
 

examples/highcmd_development.cpp

@@ -1,6 +1,7 @@
 #include "unitree_arm_sdk/control/unitreeArm.h"
 
-int main(){
+int main()
+{
     UNITREE_ARM::unitreeArm arm(true);
     arm.sendRecvThread->start();
     arm.backToStart();
@@ -11,11 +12,12 @@ int main(){
     lastPos = arm.lowstate->getQ();
     targetPos << 0.0, 1.5, -1.0, -0.54, 0.0, 0.0;
 
+    UNITREE_ARM::Timer timer(arm._ctrlComp->dt);
     for(int i=0; i<duration; i++)
     {
         arm.q = lastPos*(1-i/duration) + targetPos*(i/duration);
         arm.qd = (targetPos-lastPos)/(duration*arm._ctrlComp->dt);
-        usleep(2000);
+        timer.sleep();
     }
 
     arm.backToStart();

examples/lowcmd_development.cpp

@@ -2,38 +2,10 @@
 
 using namespace UNITREE_ARM;
 
-class Z1ARM : public unitreeArm{
-public:
-    Z1ARM():unitreeArm(true){
-        runThread = new LoopFunc("Z1LowCmd", 0.002, boost::bind(&Z1ARM::run, this));
-    };
-    ~Z1ARM(){delete runThread;};
-    void run();
-    LoopFunc *runThread;
-
-    double direction;
-    double velocity = 0.5;
-};
-
-void Z1ARM::run(){
-    tau(0) = direction*3.;// torque
-    q(1) += direction*_ctrlComp->dt*velocity;// hybrid, q & qd
-    qd(1) = direction*velocity;
-    q(2) -= direction*_ctrlComp->dt*velocity;
-    qd(2) = direction*velocity;
-    q(4) += direction*_ctrlComp->dt*velocity;// position
-    qd(5) = direction*1.5;// velocity
-    // gripper, if arm doesn't has gripper, it does noting.
-    gripperQ -= direction*_ctrlComp->dt*velocity;
-    Vec6 gTemp = _ctrlComp->armModel->inverseDynamics(q, qd, Vec6::Zero(), Vec6::Zero());
-    gTemp(0) = tau(0);
-    tau = gTemp;
-    sendRecv();
-}
-
 int main(int argc, char *argv[]) {
     std::cout << std::fixed << std::setprecision(3);
-    Z1ARM arm;
+    bool hasGripper = true;
+    unitreeArm arm(hasGripper);
     arm.sendRecvThread->start();
 
     arm.backToStart();
@@ -43,26 +15,27 @@ int main(int argc, char *argv[]) {
     std::vector<double> KP, KW;
     KP = arm._ctrlComp->lowcmd->kp;
     KW = arm._ctrlComp->lowcmd->kd;
-
-    // torque, only T
-    KP.at(0) = 0.0;
-    KW.at(0) = 0.0;
-    // position, only kp
-    KW.at(4) = 0.0;
-    // velocity, only kd
-    KP.at(5) = 0.0;
     arm._ctrlComp->lowcmd->setControlGain(KP, KW);
     arm.sendRecvThread->shutdown();
-    arm.runThread->start();// using runThread instead of sendRecvThread
 
-    for(int i(0); i<1000; i++){
-        // The robot arm will have vibration due to the rigid impact of the speed
-        // when the direction changes
-        arm.direction = i < 600 ? 1. : -1.;
-        usleep(arm._ctrlComp->dt*1000000);
+    Vec6 initQ = arm.lowstate->getQ();
+
+    double duration = 1000;
+    Vec6 targetQ;
+    targetQ << 0, 1.5, -1, -0.54, 0, 0;
+    Timer timer(arm._ctrlComp->dt);
+    for(int i(0); i<duration; i++){
+        arm.q = initQ * (1-i/duration) + targetQ * (i/duration);
+        arm.qd = (targetQ - initQ) / (duration * arm._ctrlComp->dt);
+        arm.tau = arm._ctrlComp->armModel->inverseDynamics(arm.q, arm.qd, Vec6::Zero(), Vec6::Zero());
+        arm.gripperQ = -(i/duration);
+        
+        arm.setArmCmd(arm.q, arm.qd, arm.tau);
+        arm.setGripperCmd(arm.gripperQ, arm.gripperW, arm.gripperTau);
+        arm.sendRecv();
+        timer.sleep();
     }
 
-    arm.runThread->shutdown();
     arm.sendRecvThread->start();
 
     arm.setFsm(ArmFSMState::JOINTCTRL);

include/unitree_arm_sdk/message/udp.h

@@ -6,7 +6,7 @@
 #include <string>
 #include <string.h>
 #include <vector>
-#include "unitree_arm_sdk/utilities/timer.h"
+#include "unitree_arm_sdk/utilities/loop.h"
 #include <chrono>
 
 namespace UNITREE_ARM {

include/unitree_arm_sdk/utilities/loop.h

@@ -1,59 +1,153 @@
-#ifndef _UNITREE_ARM_LOOP_H_
-#define _UNITREE_ARM_LOOP_H_
+#pragma once
 
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
+#include <chrono>
 #include <thread>
-#include <pthread.h>
-#include <vector>
-#include <boost/shared_ptr.hpp>
-#include <boost/function.hpp>
-#include <boost/bind.hpp>
-#include "unitree_arm_sdk/utilities/timer.h"
+#include <functional>
+#include <iostream>
+#include <memory>
+#include <sys/time.h>
+#include <sys/timerfd.h>
+#include <string.h>
+#include <time.h>
+#include <unistd.h>
+// #include <spdlog/spdlog.h>
 
 namespace UNITREE_ARM {
-typedef boost::function<void ()> Callback;
 
-class Loop {
-public:
-  Loop(std::string name, float period, int bindCPU = -1);
-  ~Loop();
-  void start();
-  void shutdown();
-  virtual void functionCB() = 0;
-
-private:
-  void entryFunc();
-
-  std::string _name;
-  float _period;
-  int _bindCPU;
-  bool _bind_cpu_flag = false;
-  bool _isrunning = false;
-  std::thread _thread;
-
-  size_t _runTimes = 0;
-  size_t _timeOutTimes = 0;
-
-  AbsoluteTimer *_timer;
-};
-
-class LoopFunc : public Loop {
-public:
-/*
- * Function: create a thead run once every period
- * Input: name: indicate what the thread aims to
- *        period : time, unit: second
- *        _cb : the function pointer
+/**
+ * @brief A timer to ensure the frequency of thread execution.
  */
-  LoopFunc(std::string name, float period, const Callback& _cb)
-    : Loop(name, period), _fp(_cb){}
-  LoopFunc(std::string name, float period, int bindCPU, const Callback& _cb)
-    : Loop(name, period, bindCPU), _fp(_cb){}
-  void functionCB() { (_fp)(); }
+class Timer
+{
+public:
+  /**
+   * @brief Construct a new Timer object
+   * 
+   * @param period_sec time, Unit: second
+   */
+  Timer(double period_sec) {
+    period_ = period_sec;
+    start(); // default
+  }
+
+  double period() {return period_; }
+
+  /**
+   * @brief Update the beginning time.
+   */
+  void start() { start_time_ = std::chrono::steady_clock::now();  }
+
+  /**
+   * @brief Caculate the time from the beginning to the present.
+   * 
+   * @return second
+   */
+  double elasped_time() 
+  {
+    auto end_time_ = std::chrono::steady_clock::now();
+    auto elasped_time = end_time_ - start_time_;
+    size_t t = std::chrono::duration_cast<std::chrono::microseconds>(elasped_time).count();
+    return (double)t/1000000.0;
+  }
+
+  /**
+   * @brief Caculate the remaining time to a period
+   * 
+   * @return second
+   */
+  double wait_time() { return period_ - elasped_time(); }
+
+  /**
+   * @brief Sleep for wait_time() until a period finished.
+   * 
+   * If it has timeout, do nothing.
+   */
+  void sleep()
+  {
+    double waitTime = wait_time();
+    if(waitTime > 0) {
+      std::this_thread::sleep_for(std::chrono::microseconds(size_t(waitTime*1000000)));
+    }
+    start();// If the last one ends and then start a new timer.
+  }
+
 private:
-  boost::function<void ()>  _fp;
+  double period_;
+  std::chrono::steady_clock::time_point start_time_;
 };
-}
-#endif  // _UNITREE_ARM_LOOP_H_
+typedef std::shared_ptr<Timer> TimerPtr;
+
+
+/**
+ * @brief Maintains a thread to run once every period.
+ */
+class LoopFunc
+{
+public:
+  /**
+   * @brief Construct a new Loop object
+   * 
+   * @param name Indicate what the loop aims to
+   * @param period time, Unit: second
+   * @param callback the running function pointer
+   */
+  LoopFunc(std::string name, double period, std::function<void ()> callback) 
+    :name_(name), cb_(callback) {
+    timer_ = std::make_shared<Timer>(period);
+  }
+  ~LoopFunc() { shutdown(); }
+  
+  void start()
+  {
+    if(isrunning_)
+    {
+      // spdlog::warn("Loop {} is already running.", name_);
+      return;
+    }
+
+    isrunning_ = true;
+    thread_ = std::thread(&LoopFunc::running_impl, this);
+  }
+
+  void shutdown()
+  {
+    if(!isrunning_) return;
+    isrunning_ = false;
+    thread_.join();
+    // spdlog::debug("[Report] The time out rate of thread {0} is {1}%",
+    //                name_, 100.0*(double)timeout_times_/(double)run_times_);
+  }
+
+  void spinOnce()
+  {
+    timer_->start();
+    ++run_times_;
+    cb_();
+
+    if(timer_->wait_time() > 0) {
+      timer_->sleep();
+    } else {
+      ++timeout_times_;
+      // spdlog::debug("[Report] Loop {0} timeout. It has cost {1}s.", name_, timer_->elasped_time());
+    }
+  }
+private:
+  void running_impl() {
+    while (isrunning_) {
+      spinOnce();
+    }
+    // spdlog::debug("Loop {} end.", name_);
+  }
+
+  std::string name_{};
+  bool isrunning_ = false;
+  std::shared_ptr<Timer> timer_;
+
+  std::function<void ()> cb_;
+  std::thread thread_;
+
+  size_t run_times_ = 0;
+  size_t timeout_times_ = 0;
+};
+
+}

include/unitree_arm_sdk/utilities/timer.h

@@ -1,54 +0,0 @@
-#ifndef _UNITREE_ARM_TIMER_H_
-#define _UNITREE_ARM_TIMER_H_
-
-#include <iostream>
-#include <unistd.h>
-#include <stdint.h>
-#include <sys/time.h>
-#include <sys/timerfd.h>
-
-namespace UNITREE_ARM {
-
-inline long long getSystemTime(){
-    struct timeval t;  
-    gettimeofday(&t, NULL);
-    return 1000000 * t.tv_sec + t.tv_usec;  
-}
-
-inline double getTimeSecond(){
-    double time = getSystemTime() * 0.000001;
-    return time;
-}
-
-inline void absoluteWait(long long startTime, long long waitTime){
-    if(getSystemTime() - startTime > waitTime){
-        std::cout << "[WARNING] The waitTime=" << waitTime << " of function absoluteWait is not enough!" << std::endl
-        << "The program has already cost " << getSystemTime() - startTime << "us." << std::endl;
-    }
-    while(getSystemTime() - startTime < waitTime){
-        usleep(50);
-    }
-}
-
-
-/*
-waitTimeS = 0 means do not care time out
-*/
-class AbsoluteTimer{
-public:
-    AbsoluteTimer(double waitTimeS);
-    ~AbsoluteTimer();
-    void start();
-    bool wait();
-private:
-    void _updateWaitTime(double waitTimeS);
-    int _timerFd;
-    uint64_t _missed;
-    double _waitTime;
-    double _startTime;
-    double _leftTime;
-    double _nextWaitTime;
-    itimerspec _timerSpec;
-};
-}
-#endif