V2.2.0

1. delete example_CtrlByFSM
2. add wait flag
3. change cartesian command from endposture to spatial twist
4. add jointCtrlCmd() and cartesianCtrlCmd()

CMakeLists.txt

@@ -4,7 +4,7 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3 -pthread")
 
 set(EIGEN_PATH /usr/include/eigen3)
 include_directories(
-    include/unitree_arm_sdk
+    include
     ${EIGEN_PATH}
 )
 
@@ -16,7 +16,5 @@ 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_JointCtrl examples/example_JointCtrl.cpp )
 target_link_libraries(example_JointCtrl Z1_SDK_Linux64)

examples/bigDemo.cpp

@@ -1,4 +1,4 @@
-#include "control/unitreeArm.h"
+#include "unitree_arm_sdk/control/unitreeArm.h"
 
 using namespace UNITREE_ARM;
 
@@ -7,11 +7,9 @@ public:
     Z1ARM():unitreeArm(true){};
     ~Z1ARM(){};
     void armCtrlByFSM();
-    void armCtrlTrackInJointCtrl();
+    void armCtrlInJointCtrl();
-    void armCtrlTrackInCartesian();
+    void armCtrlInCartesian();
     void printState();
-private:
-    Vec6 qPast;
 };
 
  
@@ -30,7 +28,13 @@ void Z1ARM::armCtrlByFSM() {
 
     std::cout << "[MOVEL]" << std::endl;
     posture[0] << 0,0,0,0.45,-0.2,0.2;
+    setWait(false);
     MoveL(posture[0], 0., 0.3);
+    //check trajectory finish
+    while(_ctrlComp->recvState.state != ArmFSMState::JOINTCTRL){
+        usleep(2000);
+    }
+    setWait(true);
     
     std::cout << "[MOVEC]" << std::endl;
     posture[0] << 0,0,0,0.45,0,0.4;
@@ -38,39 +42,26 @@ void Z1ARM::armCtrlByFSM() {
     MoveC(posture[0], posture[1], -M_PI/3.0, 0.3);
 }
 
-void Z1ARM::armCtrlTrackInJointCtrl(){
+void Z1ARM::armCtrlInJointCtrl(){
     labelRun("forward");
     startTrack(ArmFSMState::JOINTCTRL);
-    for(;;){
-        q(3) -= _ctrlComp->dt * 1.0;//max dt*PI, rad/s
 
-        qPast = lowstate->getQ();
+    for(int i(0); i<1000;i++){
-        // std::cout << "qCmd: " << q.transpose() << " qState: " << qPast.transpose() << std::endl;
+        directions<< 0, 0, 0, -1, 0, 0, 0;
-        //The joint has reached limit, there is warning: joint cmd is far from state
+        jointCtrlCmd(directions, 0.5);
-        double error = fabs(q(3) - qPast(3));
-        if(error > 0.1){
-            break;
-        }
         usleep(_ctrlComp->dt*1000000);
     }
-    _ctrlComp->sendCmd.track = false;
 }
 
-void Z1ARM::armCtrlTrackInCartesian(){
+void Z1ARM::armCtrlInCartesian(){
     labelRun("forward");
     startTrack(ArmFSMState::CARTESIAN);
-    for(;;){
-        endPosture(5) -= _ctrlComp->dt * 0.2;//z axis, m/s
     
-        // no inverse kinematics solution, the joint has reached limit
+    for(int i(0); i<1000;i++){
-        std::cout << "postureCmd: " << endPosture.transpose() << " qState: " << lowstate->endPosture.transpose() << std::endl;
+        directions<< 0, 0, 0, 0, 0, -1, 0;
-        double error = fabs(endPosture(5) - lowstate->endPosture(5));
+        cartesianCtrlCmd(directions, 0., 0.2);
-        if( error > 0.1){
-            break;
-        }
         usleep(_ctrlComp->dt*1000000);
     }
-    _ctrlComp->sendCmd.track = false;
 }
 
 void Z1ARM::printState(){
@@ -91,7 +82,7 @@ int main() {
 
     arm.backToStart();
 
-    // size_t demo = 3;
+    // size_t demo = 2;
     for(size_t demo = 1; demo < 4; demo++)
     switch (demo)
     {
@@ -99,10 +90,10 @@ int main() {
             arm.armCtrlByFSM();
             break;
         case 2:
-            arm.armCtrlTrackInJointCtrl();
+            arm.armCtrlInJointCtrl();
             break;
         case 3:
-            arm.armCtrlTrackInCartesian();
+            arm.armCtrlInCartesian();
             break;
         default:
             break;

examples/example_CtrlByFSM.cpp

@@ -1,37 +0,0 @@
-#include "control/unitreeArm.h"
-
-using namespace UNITREE_ARM;
-
-int main() {
-    Vec6 posture[2];
-    unitreeArm arm(true);
-    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();
-    return 0;
-}

examples/example_JointCtrl.cpp

@@ -1,4 +1,4 @@
-#include "control/unitreeArm.h"
+#include "unitree_arm_sdk/control/unitreeArm.h"
 
 using namespace UNITREE_ARM;
 
@@ -6,6 +6,8 @@ class JointTraj{
 public:
     JointTraj(){};
     ~JointTraj(){};
+
+    // set _pathTime and a3, a4, a5
     void setJointTraj(Vec6 startQ, Vec6 endQ, double speed){
         _startQ = startQ;
         _endQ = endQ;
@@ -16,6 +18,7 @@ public:
         }
         _generateA345();
     };
+
     void setGripper(double startQ, double endQ, double speed){
         _gripperStartQ = startQ;
         _gripperEndQ = endQ; 
@@ -60,6 +63,10 @@ private:
         _reached = (_tCost>_pathTime) ? true : false;
         _tCost = (_tCost>_pathTime) ? _pathTime : _tCost;
     }
+
+    // caculate the coefficients of the quintuple polynomial
+    //      s(t) = a_0 + a_1*t + a_2*t^2 + a_3*t^3 + a_4*t^4 +a_5*t^5
+    //      constraints: s(0) = 0; sdot(0) = 0; sdotdot(0) = 0; s(T) = 1; sdot(T) = 0; sdotdot(T) = 0
     void _generateA345(){
         if(NearZero(_pathTime)){
             _a3 = 0;
@@ -107,6 +114,7 @@ int main(){
     forward << 0.0, 1.5, -1.0, -0.54, 0.0, 0.0;
     double speed = 0.5;
     arm.startTrack(ArmFSMState::JOINTCTRL);
+    // move from current posture to [forward]
     arm.jointTraj.setJointTraj(arm._ctrlComp->lowstate->getQ(), forward, speed);
     arm.jointTraj.setGripper(arm._ctrlComp->lowstate->getGripperQ(), -1, speed);
     

examples/example_lowCmd_send.cpp

@@ -1,4 +1,4 @@
-#include "control/unitreeArm.h"
+#include "unitree_arm_sdk/control/unitreeArm.h"
 
 using namespace UNITREE_ARM;
 

include/unitree_arm_sdk/control/ctrlComponents.h

@@ -1,12 +1,12 @@
 #ifndef CTRLCOMPONENTS_H
 #define CTRLCOMPONENTS_H
 
-#include "message/arm_common.h"
+#include "unitree_arm_sdk/message/arm_common.h"
-#include "message/LowlevelCmd.h"
+#include "unitree_arm_sdk/message/LowlevelCmd.h"
-#include "message/LowlevelState.h"
+#include "unitree_arm_sdk/message/LowlevelState.h"
-#include "message/udp.h"
+#include "unitree_arm_sdk/message/udp.h"
-#include "utilities/loop.h"
+#include "unitree_arm_sdk/utilities/loop.h"
-#include "model/ArmModel.h"
+#include "unitree_arm_sdk/model/ArmModel.h"
 
 namespace UNITREE_ARM {
 struct CtrlComponents{

include/unitree_arm_sdk/control/unitreeArm.h

@@ -1,7 +1,7 @@
 #ifndef __UNITREEARM_H
 #define __UNITREEARM_H
 
-#include "control/ctrlComponents.h"
+#include "unitree_arm_sdk/control/ctrlComponents.h"
 
 namespace UNITREE_ARM {
 
@@ -77,7 +77,7 @@ bool MoveJ(Vec6 posture, double maxSpeed);
  * Input:    posture: target position, (roll pitch yaw x y z), unit: meter
  *           gripperPos: target angular
  *             uint: radian
- *             range:[0, pi/3]
+ *             range:[-pi/2, 0]
  *           maxSpeed: the maximum joint speed when robot is moving
  *             unit: radian/s
  *             range:[0, pi]
@@ -95,7 +95,7 @@ bool MoveL(Vec6 posture, double maxSpeed);
  * Function: Move the robot in a linear path, and control the gripper at the same time
  * Input:    posture: target position, (roll pitch yaw x y z), unit: meter
  *           gripperPos: target angular, uint: radian
- *             range:[0, pi/3]
+ *             range:[-pi/2, 0]
  *           maxSpeed: the maximum joint speed when robot is moving, unit: m/s
  * Output:   whether posture has inverse kinematics
  */
@@ -113,7 +113,7 @@ bool MoveC(Vec6 middlePosutre, Vec6 endPosture, double maxSpeed);
  * Input:    middle posture: determine the shape of the circular path
  *           endPosture: target position, (roll pitch yaw x y z), unit: meter
  *           gripperPos: target angular, uint: radian
- *             range:[0, pi/3]
+ *             range:[-pi/2, 0]
  *           maxSpeed: the maximum joint speed when robot is moving, unit: m/s
  * Output:   whether posture has inverse kinematics
  */
@@ -131,11 +131,10 @@ bool MoveC(Vec6 middlePosutre, Vec6 endPosture, double gripperPos, double maxSpe
  *                  gripperQd: <----  lowstate->getGripperQd()
  *              then you can change these parameters to control robot
  *              2. ArmFSMState::CARTESIAN, 
- *              if you run function startTrack(ArmFSMState::JOINTCTRL), 
+ *              if you run function startTrack(ArmFSMState::CARTESIAN), 
  *              firstly, the following parameters will be set at the first time:
- *                  endPosture: <---- lowstate->endPosture;
+ *                  twist.setZero()
- *                  gripperQ:   <---- lowstate->getGripperQ()
+ *              then you can change it to control robot
- *              then you can change these parameters to control robot
  */
 void startTrack(ArmFSMState fsm);
 /*
@@ -153,15 +152,74 @@ void startTrack(ArmFSMState fsm);
  */
 void sendRecv();
 
+/*
+ * Function: whether to wait for the command to finish
+ * Input:    true or false
+ * Output:   None
+ * Description: For example, MoveJ will send a trajectory command to z1_controller and then
+ *              run usleep() to wait for the trajectory execution to complete.
+ *              If set [wait] to false, MoveJ  will send command only and user should judge 
+ *              for youself whether the command is complete.
+ *                  Method 1: if(_ctrlComp->recvState.state != fsm)
+ *                      After trajectory complete, the FSM will switch to ArmFSMState::JOINTCTRL
+ *                      automatically
+ *                  Method 2: if((lowState->endPosture - endPostureGoal).norm() < error)
+ *                      Check whether current posture reaches the target
+ *              Related functions:
+ *                  MoveJ(), MoveL(), MoveC(), backToStart(), labelRun(), teachRepeat()
+ */
+void setWait(bool Y_N);
+
+/*
+ * Function: set q & qd command automatically by input parameters
+ * Input:    directions: movement directions [include gripper], range:[-1,1]
+ *                       J1, J2, J3, J4, J5, J6, gripper
+ *           jointSpeed: range: [0, pi]
+ * Output:   None
+ * Description: The function is typically used to control the robot by keyboard or joystick
+ *              When a key is pressed, the directions[i] sets to 1 or -1, and the function will 
+ *              automatically execute the following command:
+ *                      qd = directions * jointSpeed
+ *                      q += qd * _ctrlComp->dt
+ *              if directions == 0, the robot stop moving
+ */
+void jointCtrlCmd(Vec7 directions, double jointSpeed);
+
+/*
+ * Function: set spatial velocity command automatically by input parameters
+ * Input:    directions: movement directions [include gripper], range:[-1,1]
+ *                       raw, pitch, yaw, x, y, z, gripper
+ *           oriSpeed: range: [0, 0.6]
+ *           posSpeed: range: [0, 0.3]
+ *                  gripper joint speed is set to 1.0
+ * Output:   None
+ * Description: The function is typically used to control the robot by keyboard or joystick
+ *              When a key is pressed, the directions[i] is set to 1 or -1, and the function will 
+ *              automatically execute the following command:
+ *                      Tdelta = directions * speed
+ *                      Tgoal  = Tdelta * Tpast
+ *                      omega  = so3ToVec(MatrixLog3( Tpast.Rot.Transpose * Tgoal.Rot ))
+ *                      v      = Tdelta.t
+ *                      twist  = (omega, v)
+ *              if directions == 0, the robot stop moving
+ */
+void cartesianCtrlCmd(Vec7 directions, double oriSpeed, double posSpeed);
+
 //command parameters
 Vec6 q, qd, tau;
-Vec6 endPosture;
+Vec6 twist;//spatial velocity: [omega, v]'
 double gripperQ, gripperW, gripperTau;
+Vec7 directions;
 
 LoopFunc *sendRecvThread;
 LowlevelCmd *lowcmd;//same as _ctrlComp->lowcmd
 LowlevelState *lowstate;//same as _ctrlComp->lowstate
 CtrlComponents *_ctrlComp;
+
+private:
+    bool _isWait = true;
+    Vec6 _qPast;
+    Vec6 _endPosturePast, _endPostureDelta, _endPostureGoal;
 };
 
 }

include/unitree_arm_sdk/math/mathTools.h

@@ -3,7 +3,7 @@
 
 #include <iostream>
 #include <math.h>
-#include "math/mathTypes.h"
+#include "unitree_arm_sdk/math/mathTypes.h"
 
 namespace UNITREE_ARM {
 

include/unitree_arm_sdk/math/mathTypes.h

@@ -18,6 +18,9 @@ using Vec4 = typename Eigen::Matrix<double, 4, 1>;
 // 6x1 Vector
 using Vec6 = typename Eigen::Matrix<double, 6, 1>;
 
+// 7x1 Vector
+using Vec7 = typename Eigen::Matrix<double, 7, 1>;
+
 // Quaternion
 using Quat = typename Eigen::Matrix<double, 4, 1>;
 

include/unitree_arm_sdk/message/LowlevelCmd.h

@@ -1,7 +1,7 @@
 #ifndef LOWLEVELCMD_H
 #define LOWLEVELCMD_H
 
-#include "math/mathTypes.h"
+#include "unitree_arm_sdk/math/mathTypes.h"
 #include <vector>
 #include <iostream>
 
@@ -10,7 +10,7 @@ struct LowlevelCmd{
 private:
     size_t _dof = 6;
 public:
-    Vec6 endPosture;
+    Vec6 twsit;
     std::vector<double> q;
     std::vector<double> dq;
     std::vector<double> tau;

include/unitree_arm_sdk/message/LowlevelState.h

@@ -3,8 +3,8 @@
 
 #include <iostream>
 #include <vector>
-#include "math/mathTypes.h"
+#include "unitree_arm_sdk/math/mathTypes.h"
-#include "message/arm_common.h"
+#include "unitree_arm_sdk/message/arm_common.h"
 
 namespace UNITREE_ARM {
 

include/unitree_arm_sdk/message/arm_common.h

@@ -83,7 +83,7 @@ struct TrajCmd{
     double gripperPos;
     double maxSpeed;
     double stopTime;
-    int trajOrder;
+    int reserved;
 };
 
 union ValueUnion{

include/unitree_arm_sdk/message/udp.h

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

include/unitree_arm_sdk/model/ArmModel.h

@@ -2,8 +2,8 @@
 #define ARMMODEL_H
 
 #include <vector>
-#include "thirdparty/robotics.h"
+#include "unitree_arm_sdk/thirdparty/robotics.h"
-#include "thirdparty/quadProgpp/QuadProg++.hh"
+#include "unitree_arm_sdk/thirdparty/quadProgpp/QuadProg++.hh"
 
 
 namespace UNITREE_ARM {

include/unitree_arm_sdk/thirdparty/quadProgpp/QuadProg++.hh

@@ -62,7 +62,7 @@ s.t.
 #ifndef _QUADPROGPP
 #define _QUADPROGPP
 
-#include "thirdparty/quadProgpp/Array.hh"
+#include "unitree_arm_sdk/thirdparty/quadProgpp/Array.hh"
 #include <eigen3/Eigen/Dense>
 
 namespace quadprogpp {

include/unitree_arm_sdk/thirdparty/robotics.h

@@ -1,7 +1,7 @@
 #pragma once
 
 #include <vector>
-#include "math/mathTools.h"
+#include "unitree_arm_sdk/math/mathTools.h"
 
 namespace robo {
 /*

include/unitree_arm_sdk/utilities/loop.h

@@ -10,7 +10,7 @@
 #include <boost/shared_ptr.hpp>
 #include <boost/function.hpp>
 #include <boost/bind.hpp>
-#include "utilities/timer.h"
+#include "unitree_arm_sdk/utilities/timer.h"
 
 namespace UNITREE_ARM {
 typedef boost::function<void ()> Callback;