V2.2.6 add gripper tau limit interface

Author: Agnel-Wang

config/config.xml

@@ -8,4 +8,6 @@
         <limitT>10.0</limitT> <!-- N*M -->
         <load>0.0</load><!-- kg-->
     </collision>
+
+    <grasp_max_torque>10.0</grasp_max_torque>
 </configurartion>

include/FSM/FSMState.h

@@ -8,6 +8,7 @@
 #include "common/math/mathTools.h"
 #include "common/utilities/timer.h"
 #include "FSM/BaseState.h"
+#include "model/unitree_gripper.h"
 
 class FSMState : public BaseState{
 public:
@@ -25,11 +26,13 @@ class FSMState : public BaseState{
     void _recordData();
     Vec6 _postureToVec6(Posture posture);
     void _tauFriction();
+    void _zero_position_joint4_protection();
 
     LowlevelCmd *_lowCmd;
     LowlevelState *_lowState;
     IOInterface *_ioInter;
     ArmModel *_armModel;
+    std::shared_ptr<Unitree_Gripper> _gripper;
 
     Vec6 _qPast, _qdPast, _q, _qd, _qdd, _tauForward;
     double _gripperPos, _gripperW, _gripperTau;

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/control/CtrlComponents.h

@@ -12,6 +12,7 @@
 #include "interface/IOUDP.h"
 #include "interface/IOROS.h"
 #include "control/armSDK.h"
+#include "model/unitree_gripper.h"
 
 using namespace std;
 
@@ -25,6 +26,7 @@ struct CtrlComponents{
     IOInterface *ioInter;
     Z1Model *armModel;
     CSVTool *stateCSV;
+    std::shared_ptr<Unitree_Gripper> gripper;
 
     SendCmd sendCmd; // cmd that receive from SDK 
     RecvState recvState;// state that send to SDK

include/control/armSDK.h

@@ -7,11 +7,12 @@
 class ARMSDK : public CmdPanel{
 public:
     ARMSDK(std::vector<KeyAction*> events, 
-        EmptyAction emptyAction, const char* IP, uint toport, uint ownport, double dt = 0.002);
+        EmptyAction emptyAction, const char* IP, uint port, double dt = 0.002);
     ~ARMSDK();
     SendCmd getSendCmd();
     int getState(size_t channelID = 0);
     void setRecvState(RecvState& recvState);
+    void start();
 private:
     void _sendRecv();
     void _read(){};

include/control/cmdPanel.h

@@ -76,7 +76,7 @@ class ValueAction : public KeyAction{
     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 _dVdtf = 0.0; // delta value per second after fliter
     double _lim1, _lim2;
     bool _hasLim = false;
     bool _hasGoZero = false;
@@ -104,6 +104,7 @@ class CmdPanel{
     virtual std::vector<std::vector<double> > stringToMatrix(std::string slogan);
     virtual SendCmd getSendCmd();
     virtual void setRecvState(RecvState& recvState){};
+    void start(){_runThread->start();}
 protected:
     virtual void _read() = 0;
     void _run();

include/model/unitree_gripper.h

@@ -0,0 +1,109 @@
+#ifndef _UNITREE_GRIPPER_H
+#define _UNITREE_GRIPPER_H
+
+#include <algorithm>
+#include <cmath>
+#include <chrono>
+#include "common/math/mathTools.h"
+
+typedef struct
+{
+  double angle;                      	// Current gripper opening angle. Unit: rad
+  double speed;												// Current measured gripper speed. Unit: rad/s
+  double tau;													// Current measured gripper output torque. Unit: Nm
+  bool   is_grasped;									// Indicates whether an object is currently grasped.
+  int8_t temperature;                	// current temperature (temperature conduction is slow that leads to lag)
+}GripperState;
+
+typedef struct
+{
+  double angle;                       // Size of object to grasp in radian               
+  double speed;                       // Closing speed in rad/s          
+  double tau;                         // Grasping tau in Nm	
+  double epsilon_inner;               // Maximum tolerated deviation when the actual grasped angle is smaller than the commanded grasp angle.
+  double epsilon_outer;               // Maximum tolerated deviation when the actual grasped angle is larger than the commanded grasp angle.
+}GripperCmd;
+
+class Unitree_Gripper
+{
+public:
+  Unitree_Gripper();
+  Unitree_Gripper(double max_toque);
+  ~Unitree_Gripper() = default;
+
+  /**
+   * @brief Move the gripper to target angle
+   * 
+   * @param angle Target opening angle in rad
+   * @param speed Opening speed in rad/s
+   */
+  void move(double angle, double speed);
+
+  /**
+   * @brief Grasps an object
+   * 
+   * @param angle Size of object to grasp in radian.
+   * @param speed Closing speed in rad/s.
+   * @param tau Grasping torque in Nm
+   * @param epsilon_inner Maximum tolerated deviation when the actual grasped angle 
+   * is smaller than the commanded grasp angle.
+   * @param epsilon_outer Maximum tolerated deviation when the actual grasped angle 
+   * is larger than the commanded grasp angle.
+   * @return True if an object has been grasped.
+   */
+  void grasp( double angle,
+              double speed,
+              double tau,
+              double epsilon_inner = 0.01,
+              double epsilon_outer = 0.01);
+  
+  /**
+   * @brief Get current output torque due to current state
+   * 
+   * @param angle Measured gripper angle
+   * @param speed Measured gripper speed
+   * @param torque Measured gripper torque
+   * @param temperature Measured gripper temperature
+   * @return double Output torque
+   */
+  double update(double angle, double speed, double torque, int8_t temperature = 0);
+
+  GripperCmd cmd{};
+  GripperState state{};
+private:
+  enum class GripperCtrlMode{
+    Move,// Position
+    Grasp// Torque
+  } mode = GripperCtrlMode::Move;
+
+  struct
+  {
+    double kp_p;
+    double kd_p;
+    double kp_v;
+    double ki_v;
+    double kd_v;
+  } coe{};
+  
+  typedef struct
+  {
+    double angle;
+    double speed;
+  } Error_state;
+
+  Error_state error{}, error_last{};
+  double error_speed_sum_{};
+
+  static constexpr double MAX_POSITION = -M_PI/2;
+  static constexpr double MAX_SPEED = M_PI;
+  double MAX_TORQUE = 20.0;
+
+  std::chrono::steady_clock::time_point first_grasped_time_{};
+  bool is_stopped_{};
+  double tau_output{};
+
+  void modify_cmd(GripperCmd& gripper_cmd);
+  bool is_grasped();
+};
+
+#endif // _UNITREE_GRIPPER_H