Revert "add wait flag"

This reverts commit be77f00cfb.

examples/bigDemo.cpp

@@ -1,4 +1,4 @@
-#include "unitree_arm_sdk/control/unitreeArm.h"
+#include "control/unitreeArm.h"
 
 using namespace UNITREE_ARM;
 
@@ -63,7 +63,7 @@ void Z1ARM::armCtrlTrackInCartesian(){
         endPosture(5) -= _ctrlComp->dt * 0.2;//z axis, m/s
 
         // no inverse kinematics solution, the joint has reached limit
-        // std::cout << "postureCmd: " << endPosture.transpose() << " qState: " << lowstate->endPosture.transpose() << std::endl;
+        std::cout << "postureCmd: " << endPosture.transpose() << " qState: " << lowstate->endPosture.transpose() << std::endl;
         double error = fabs(endPosture(5) - lowstate->endPosture(5));
         if( error > 0.1){
             break;
@@ -91,7 +91,7 @@ int main() {
 
     arm.backToStart();
 
-    // size_t demo = 1;
+    // size_t demo = 3;
     for(size_t demo = 1; demo < 4; demo++)
     switch (demo)
     {

examples/example_CtrlByFSM.cpp

@@ -1,4 +1,4 @@
-#include "unitree_arm_sdk/control/unitreeArm.h"
+#include "control/unitreeArm.h"
 
 using namespace UNITREE_ARM;
 
@@ -16,23 +16,17 @@ int main() {
     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.setWait(false);
     arm.MoveL(posture[0], 0., 0.3);
-    while(arm._ctrlComp->recvState.state != ArmFSMState::JOINTCTRL){
-        usleep(2000);
-    }
-
+    
     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.setWait(true);
     arm.MoveC(posture[0], posture[1], -M_PI/3.0, 0.3);
 
     //stop

examples/example_JointCtrl.cpp

@@ -1,4 +1,4 @@
-#include "unitree_arm_sdk/control/unitreeArm.h"
+#include "control/unitreeArm.h"
 
 using namespace UNITREE_ARM;
 
@@ -6,8 +6,6 @@ class JointTraj{
 public:
     JointTraj(){};
     ~JointTraj(){};
-
-    // set _pathTime and a3, a4, a5
     void setJointTraj(Vec6 startQ, Vec6 endQ, double speed){
         _startQ = startQ;
         _endQ = endQ;
@@ -18,7 +16,6 @@ public:
         }
         _generateA345();
     };
-
     void setGripper(double startQ, double endQ, double speed){
         _gripperStartQ = startQ;
         _gripperEndQ = endQ; 
@@ -63,10 +60,6 @@ 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;
@@ -114,7 +107,6 @@ 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 "unitree_arm_sdk/control/unitreeArm.h"
+#include "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 "unitree_arm_sdk/message/arm_common.h"
-#include "unitree_arm_sdk/message/LowlevelCmd.h"
-#include "unitree_arm_sdk/message/LowlevelState.h"
-#include "unitree_arm_sdk/message/udp.h"
-#include "unitree_arm_sdk/utilities/loop.h"
-#include "unitree_arm_sdk/model/ArmModel.h"
+#include "message/arm_common.h"
+#include "message/LowlevelCmd.h"
+#include "message/LowlevelState.h"
+#include "message/udp.h"
+#include "utilities/loop.h"
+#include "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 "unitree_arm_sdk/control/ctrlComponents.h"
+#include "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:[-pi/2, 0]
+ *             range:[0, pi/3]
  *           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:[-pi/2, 0]
+ *             range:[0, pi/3]
  *           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:[-pi/2, 0]
+ *             range:[0, pi/3]
  *           maxSpeed: the maximum joint speed when robot is moving, unit: m/s
  * Output:   whether posture has inverse kinematics
  */
@@ -153,24 +153,6 @@ 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);
-
 //command parameters
 Vec6 q, qd, tau;
 Vec6 endPosture;
@@ -180,9 +162,6 @@ LoopFunc *sendRecvThread;
 LowlevelCmd *lowcmd;//same as _ctrlComp->lowcmd
 LowlevelState *lowstate;//same as _ctrlComp->lowstate
 CtrlComponents *_ctrlComp;
-
-private:
-    bool _isWait = true;
 };
 
 }

include/unitree_arm_sdk/math/mathTools.h

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

include/unitree_arm_sdk/message/LowlevelCmd.h

@@ -1,7 +1,7 @@
 #ifndef LOWLEVELCMD_H
 #define LOWLEVELCMD_H
 
-#include "unitree_arm_sdk/math/mathTypes.h"
+#include "math/mathTypes.h"
 #include <vector>
 #include <iostream>
 

include/unitree_arm_sdk/message/LowlevelState.h

@@ -3,8 +3,8 @@
 
 #include <iostream>
 #include <vector>
-#include "unitree_arm_sdk/math/mathTypes.h"
-#include "unitree_arm_sdk/message/arm_common.h"
+#include "math/mathTypes.h"
+#include "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 reserved;
+    int trajOrder;
 };
 
 union ValueUnion{

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 "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 "unitree_arm_sdk/thirdparty/robotics.h"
-#include "unitree_arm_sdk/thirdparty/quadProgpp/QuadProg++.hh"
+#include "thirdparty/robotics.h"
+#include "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 "unitree_arm_sdk/thirdparty/quadProgpp/Array.hh"
+#include "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 "unitree_arm_sdk/math/mathTools.h"
+#include "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 "unitree_arm_sdk/utilities/timer.h"
+#include "utilities/timer.h"
 
 namespace UNITREE_ARM {
 typedef boost::function<void ()> Callback;