V2.0.0

CMakeLists.txt

@@ -11,36 +11,17 @@ set(COMMUNICATION UDP)            # UDP
 # ----------------------configurations----------------------
 # COMMUNICATION
 if(${COMMUNICATION} STREQUAL "UDP")
-    add_definitions(-DUDP)
     set(SIMULATION OFF)
-    set(REAL_ROBOT ON) 
 elseif(${COMMUNICATION} STREQUAL "ROS")
-    add_definitions(-DROS)
+    add_definitions(-DCOMPILE_WITH_SIMULATION)
     set(SIMULATION ON)
-    set(REAL_ROBOT OFF) 
 else()
     message(FATAL_ERROR "[CMake ERROR] The COMMUNICATION is error")
 endif()
 
-# SIM OR REAL
-if(((SIMULATION) AND (REAL_ROBOT)) OR ((NOT SIMULATION) AND (NOT REAL_ROBOT)))
-    message(FATAL_ERROR "[CMake ERROR] The SIMULATION and REAL_ROBOT can only be one ON one OFF")
-endif()
-
-if(REAL_ROBOT)
-    add_definitions(-DCOMPILE_WITH_REAL_ROBOT)
-endif()
 
 if(SIMULATION)
-    add_definitions(-DCOMPILE_WITH_SIMULATION)
-    add_definitions(-DRUN_ROS)
     find_package(gazebo REQUIRED)
-endif()
-
-# ARM_MODEL
-add_definitions(-DARM_Z1)
-
-if(SIMULATION)
     find_package(catkin REQUIRED COMPONENTS
         controller_manager
         genmsg
@@ -67,24 +48,19 @@ if(SIMULATION)
     )
 
     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
-
 endif()
 
 set(EIGEN_PATH /usr/include/eigen3)
 include_directories(
     include
-    thirdparty
-    ../z1_sdk/include
     ${Boost_INCLUDE_DIR}
     ${EIGEN_PATH}
 )
 
 link_directories(lib)
 
+
 # ----------------------add executable----------------------
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/../build)                # EXECUTABLE:CMAKE_RUNTIME_OUTPUT_DIRECTORY
 add_executable(z1_ctrl main.cpp)
-# add_dependencies(z1_ctrl  Z1_${COMMUNICATION}_Linux64)
+target_link_libraries(z1_ctrl libZ1_${COMMUNICATION}_Linux64.so ${catkin_LIBRARIES} )
-target_link_libraries(z1_ctrl ${catkin_LIBRARIES} libUnitree_motor_SDK_Linux64_EPOLL.so libZ1_${COMMUNICATION}_Linux64.so)
-
-# set(EXECUTABLE_OUTPUT_PATH /home/$ENV{USER}/)

config.xml

@@ -1,26 +1,17 @@
 <configurartion>
-    <control set="2">
+    <control set=2>
         <option1>keyboard</option1>
         <option2>sdk</option2>
         <option3>joystick</option3> <!-- default aliengo_joystick -->
     </control>
 
-    <gripper set="2">
+    <!-- slave computer communication -->
-        <option1 name="none"/>
-        <option2 name="Unitree_gripper"/>
-        <option3 name="user_gripper">
-            <endPosLocalX>0.0</endPosLocalX>
-            <endPosLocalY>0.0</endPosLocalY>
-            <endPosLocalZ>0.0</endPosLocalZ>
-            <mass>0.0</mass>
-            <comX>0.0</comX>
-            <comY>0.0</comY>
-            <comZ>0.0</comZ>
-            <Ixx>0.0</Ixx>
-            <Iyy>0.0</Iyy>
-            <Izz>0.0</Izz>
-        </option3>
-    </gripper>
-
     <IP>192.168.123.110</IP>
+    <Port>8881</Port>
+
+    <collision>
+        <open>N</open> <!-- Y or N -->
+        <limitT>10.0</limitT> <!-- N*M -->
+        <load>0.0</load><!-- kg-->
+    </collision>
 </configurartion>

config/savedArmStates.csv

@@ -1,14 +1,13 @@
-forward, -0.000019, 1.542859, -1.037883, -0.531308, 0.002487, -0.012173, 0.999650, -0.002146, -0.026357, 0.389527, 0.002468, 0.999923, 0.012173, 0.000269, 0.026329, -0.012234, 0.999578, 0.402549, 0.000000, 0.000000, 0.000000, 1.000000, 
+forward, 0.0, 1.5, -1.0, -0.54, 0.0, 0.0, 
-start, -0.000336, 0.001634, 0.000000, 0.064640, 0.000248, 0.000230, 0.997805, 0.000104, 0.066225, 0.048696, -0.000087, 1.000000, -0.000252, 0.000011, -0.066225, 0.000246, 0.997805, 0.148729, 0.000000, 0.000000, 0.000000, 1.000000, 
+startFlat, 0.0, 0.0, -0.005, -0.074, 0.0, 0.0, 
-startFlat, 0.000000, 0.000000, -0.000012, -0.074351, 0.000000, 0.000006, 0.997236, -0.000000, -0.074294, 0.086594, 0.000000, 1.000000, -0.000006, 0.000000, 0.074294, 0.000006, 0.997236, 0.176788, 0.000000, 0.000000, 0.000000, 1.000000, 
+search, 0.0, 0.0, -0.005, -0.5, 0.0, 0.0, 
-teachstart, -0.858000, 1.951000, -0.825000, 0.000000, -1.154000, 0.000000, 0.653696, 0.756536, -0.018308, 0.260474, -0.756240, 0.653952, 0.021180, -0.301334, 0.027996, 0.000000, 0.999608, 0.183326, 0.000000, 0.000000, 0.000000, 1.000000, 
+show_left, -1.0, 0.9, -1., -0.3, 0.0, 0.0, 
-fulltest00, 2.316194, 0.013239, -0.001959, -0.000047, -0.278982, 0.221496, -0.654115, -0.677458, 0.336427, -0.022776, 0.708636, -0.704403, -0.040645, 0.024673, 0.264516, 0.211818, 0.940832, 0.174303, 0.000000, 0.000000, 0.000000, 1.000000, 
+show_mid, 0.0, 0.9, -1.2, 0.3, 0.0, 0.0, 
-fulltest01, 0.000194, 1.621239, -1.455959, 2.470727, 1.492796, 0.419496, 0.205219, 0.489994, 0.847225, 0.332273, 0.619814, -0.735004, 0.274956, 0.059687, 0.757440, 0.468696, -0.454542, 0.521459, 0.000000, 0.000000, 0.000000, 1.000000, 
+show_right, 1.0, 0.9, -1., -0.3, 0.0, 0.0, 
-fulltest02, -2.336194, 3.117239, -3.099593, -2.478727, -1.516796, 0.419496, 0.415340, -0.220546, 0.882526, -0.404129, -0.454627, 0.789988, 0.411380, -0.505948, -0.787913, -0.572083, 0.227847, 0.075097, 0.000000, 0.000000, 0.000000, 1.000000, 
+5kg1, 0.661051, 3.010641, -2.885593, -0.233059, 0.0, 0.0, 
-001, 0.000000, 0.922204, -1.215402, 0.293198, 0.000000, 0.000000, 1.000000, 0.000000, -0.000000, 0.200000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.500000, 0.000000, 0.000000, 0.000000, 1.000000, 
+5kg2, -0.495523, 3.122645, -2.856166, -0.406859, 0.0, 0.0, 
-002, -1.035000, 0.922204, -1.017402, -0.285802, 0.000000, 0.000000, 0.473918, 0.859862, -0.189839, 0.103971, -0.798204, 0.510526, 0.319739, -0.175114, 0.371849, 0.000000, 0.928293, 0.541400, 0.000000, 0.000000, 0.000000, 1.000000, 
+mh1, 2.617994, 0.084000, -2.755093, 1.470296, 1.454343, 2.645527, 
-003, 0.954000, 0.910204, -0.984402, -0.192802, 0.000000, 0.000000, 0.557929, -0.815736, -0.152611, 0.121384, 0.786832, 0.578425, -0.215223, 0.171185, 0.263839, 0.000000, 0.964567, 0.510707, 0.000000, 0.000000, 0.000000, 1.000000, 
+mh2, -2.485994, 2.451000, -0.102000, -1.611829, -1.421343, -2.265473, 
-mohe1, 0.966000, 1.420800, -1.412412, -1.141151, 0.997200, 1.675206, -0.560087, -0.444472, 0.699105, 0.049195, 0.666938, -0.742508, 0.062250, 0.235148, 0.491423, 0.501125, 0.712304, 0.622679, 0.000000, 0.000000, 0.000000, 1.000000, 
+01, 0.028072, 0.343213, -0.010000, -0.073124, 0.084670, -0.016248,
-mohe2, -1.057200, 2.424000, -2.682793, 0.986449, -1.342800, -1.709994, -0.765519, -0.400523, 0.503550, 0.166634, -0.625604, 0.646204, -0.437081, -0.515483, -0.150335, -0.649616, -0.745251, 0.381296, 0.000000, 0.000000, 0.000000, 1.000000, 
+02, 2.024087, 0.760020, -0.816744, -1.190535, 1.175596, 2.064632, 
-5kg1, 0.661051, 3.010641, -2.885593, -0.233059, 0.000065, 0.000206, 0.784707, -0.614016, -0.084965, 0.592966, 0.610421, 0.789294, -0.066347, 0.461215, 0.107801, 0.000198, 0.994173, 0.198126, 0.000000, 0.000000, 0.000000, 1.000000, 
+03, -1.740466, 2.731333, -2.161190, 0.806278, -0.889325, -2.529499, 
-5kg2, -0.495523, 3.122645, -2.856166, -0.406859, 0.000065, -0.000378, 0.871097, 0.475482, -0.122910, 0.664711, -0.470757, 0.879726, 0.066863, -0.359274, 0.139919, -0.000383, 0.990163, 0.133116, 0.000000, 0.000000, 0.000000, 1.000000, 

include/FSM/BaseState.h

@@ -23,10 +23,10 @@ public:
         }
     }
     std::string getStateName(){return _stateNameString;}
+    int getStateNameEnum(){return _stateNameEnum;};
 protected:
     int _stateNameEnum;
     std::string _stateNameString;
-    Control _ctrl;
 };
 
 #endif

include/FSM/FSMState.h

@@ -5,15 +5,9 @@
 #include <iostream>
 #include <unistd.h>
 #include "control/CtrlComponents.h"
-#include "message/LowlevelCmd.h"
-#include "message/LowlevelState.h"
-#include "common/enumClass.h"
 #include "common/math/mathTools.h"
-#include "common/math/mathTypes.h"
+#include "common/utilities/timer.h"
-#include "unitree_arm_sdk/timer.h"
+#include "FSM/BaseState.h"
-#include "model/ArmDynKineModel.h"
-
-#include "FSM/FiniteStateMachine.h"
 
 class FSMState : public BaseState{
 public:
@@ -24,55 +18,23 @@ public:
     virtual void run() = 0;
     virtual void exit() = 0;
     virtual int checkChange(int cmd) {return (int)ArmFSMStateName::INVALID;}
+    bool _collisionTest();
     
 protected:
     void _armCtrl();
-    void _tauDynForward();
+    void _recordData();
-    void _tauFriction();
+    Vec6 _postureToVec6(Posture posture);
-    void _qdFeedBack();
-    bool _collisionTest();
-    void _jointPosProtect();
-    void _jointSpeedProtect();
-    void _stateUpdate();
 
-    void _gripperCmd();
-    void _gripperCtrl();
-    double _gripperPos;
-    double _gripperW;
-    double _gripperTau;
-    double _gripperPosStep;//keyboard
-    double _gripperTauStep;
-
-    double _gripperLinearFriction;
-    double _gripperCoulombFriction;
-    static double _gripperCoulombDirection;
-    double _gripperFriction;
-
-    CtrlComponents *_ctrlComp;
-    ArmFSMStateName _nextStateName;
-
-    IOInterface *_ioInter;
     LowlevelCmd *_lowCmd;
     LowlevelState *_lowState;
-    UserValue _userValue;
+    IOInterface *_ioInter;
-    ArmDynKineModel *_armModel;
+    ArmModel *_armModel;
 
-    Vec6 _qPast, _qdPast, _q, _qd, _qdd;
+    Vec6 _qPast, _qdPast, _q, _qd, _qdd, _tauf, _tauCmd, _g;
-    Vec6 _tau, _endForce;
+    double _gripperPos, _gripperW, _gripperTau;
+    uint _collisionCnt;
 
-    Vec6 _coulombFriction;
+    CtrlComponents *_ctrlComp;
-    static Vec6 _coulombDirection;
-    Vec6 _linearFriction;
-
-    Vec6 _kpDiag, _kdDiag;
-    Mat6 _Kp, _Kd;
-
-    std::vector<double> _jointQMax;
-    std::vector<double> _jointQMin;
-    std::vector<double> _jointSpeedMax;
-
-    SendCmd _sendCmd;
-    RecvState _recvState;
 };
 
 #endif  // FSMSTATE_H

include/FSM/FiniteStateMachine.h

@@ -2,40 +2,26 @@
 #define FSM_H
 
 #include <vector>
-#include "FSM/BaseState.h"
+#include "FSM/FSMState.h"
-#include "unitree_arm_sdk/cmdPanel.h"
+#include "common/utilities/loop.h"
-#include "unitree_arm_sdk/loop.h"
+#include "control/CtrlComponents.h"
-#include "unitree_arm_sdk/keyboard.h"
 
-enum class FSMRunMode{
-    NORMAL,
-    CHANGE
-};
-
-/*
-    状态机初始状态为states的第一个元素
-    为了支持多个状态机从同一个CmdPanel获取触发信号，
-    加入了cmdChannel，不同状态机之间的cmdChannel必须不同，
-    否则可能会漏过状态触发
-*/
 class FiniteStateMachine{
 public:
-    FiniteStateMachine(std::vector<BaseState*> states,
+    FiniteStateMachine(std::vector<FSMState*> states, CtrlComponents *ctrlComp);
-        CmdPanel *cmdPanel, size_t cmdChannel = 0, double dt=0.002);
     virtual ~FiniteStateMachine();
 
 private:
     void _run();
-    std::vector<BaseState*> _states;
+    std::vector<FSMState*> _states;
 
-    FSMRunMode _mode;
+    FSMMode _mode;
     bool _running;
-    BaseState* _currentState;
+    FSMState* _currentState;
-    BaseState* _nextState;
+    FSMState* _nextState;
     int _nextStateEnum;
 
-    size_t _cmdChannel;
+    CtrlComponents *_ctrlComp;
-    CmdPanel *_cmdPanel;
     LoopFunc *_runThread;
 };
 

include/FSM/State_BackToStart.h

@@ -1,14 +1,21 @@
 #ifndef STATE_BACKTOSTART_H
 #define STATE_BACKTOSTART_H
 
-#include "FSM/State_Trajectory.h"
 
-class State_BackToStart : public State_Trajectory{
+#include "FSM/FSMState.h"
+#include "trajectory/JointSpaceTraj.h"
+
+class State_BackToStart : public FSMState{
 public:
     State_BackToStart(CtrlComponents *ctrlComp);
+    ~State_BackToStart();
+    void enter();
+    void run();
+    void exit();
+    int checkChange(int cmd);
 private:
-    void _setTraj();
+    bool _reach, _pastReach;
-    void _setTrajSDK(){}
+    JointSpaceTraj *_jointTraj;
 };
 
 #endif  // STATE_BACKTOSTART_H

include/FSM/State_Cartesian.h

@@ -12,17 +12,15 @@ public:
     void exit();
     int checkChange(int cmd);
 private:
-    double _posSpeed;
+    double _oriSpeed = 0.4;// control by keyboard or joystick
-    double _oriSpeed;
+    double _posSpeed = 0.15;
-    Vec3 _omega;
+    double oriSpeedLimit = 0.3;// limits in SDK
-    Vec3 _velocity;
+    double posSpeedLimit = 0.3;
+    VecX _changeDirections;
+
     Vec6 _twist;
     HomoMat _endHomoGoal, _endHomoGoalPast;
     Vec6 _endPostureGoal, _endPosturePast, _endPostureDelta;
-    HomoMat _endHomoFeedback;
-    Vec6 _Pdes;
-    Vec6 _Pfd;
-    Vec6 _Pkp;
 };
 
 #endif  // CARTESIAN_H

include/FSM/State_Dance.h

@@ -1,20 +0,0 @@
-#ifndef STATE_DANCETRAJ_H
-#define STATE_DANCETRAJ_H
-
-#include "FSM/State_Trajectory.h"
-
-class State_Dance : public State_Trajectory{
-public:
-    State_Dance(CtrlComponents *ctrlComp, 
-        TrajectoryManager *traj, 
-        ArmFSMStateName stateEnum, 
-        ArmFSMStateName nextState, 
-        std::string stateString);
-    void exit();
-    int checkChange(int cmd);
-private:
-    bool _freeBottom = false;
-    void _setTraj(){}
-};
-
-#endif

include/FSM/State_JointSpace.h

@@ -12,7 +12,7 @@ public:
     void exit();
     int checkChange(int cmd);
 private:
-    double _angleStep;
+    std::vector<double> jointSpeedMax;
 };
 
 #endif  // JOINTSPACE_H

include/FSM/State_MoveC.h

@@ -2,7 +2,7 @@
 #define MOVEC_H
 
 #include "FSM/FSMState.h"
-#include "trajectory/CartesionSpaceTraj.h"
+#include "trajectory/EndCircleTraj.h"
 
 class State_MoveC : public FSMState{
 public:
@@ -13,22 +13,10 @@ public:
     void exit();
     int checkChange(int cmd);
 private:
-    void quadprogArea();
+    double _speed;
-    double _posSpeed;
+    std::vector<Vec6>  _postures;
-    double _oriSpeed;
+    EndCircleTraj *_circleTraj;
-    Vec3 _omega;
+    bool _timeReached, _taskReached, _pastTaskReached, _finalReached;
-    Vec3 _velocity;
-    Vec6 _twist;
-    Vec6 _pastPosture, _endPosture, _middlePostureGoal, _endPostureGoal, _endTwist;
-    HomoMat _endHomoFeedback;
-    Vec6 _Pdes;
-    Vec6 _Pfd;
-    Vec6 _Pkp;
-    Vec6 _endPostureError;
-
-    // 轨迹相关变量
-    CartesionSpaceTraj *_cartesionTraj;
-    bool _reached, _timeReached, _taskReached, _pastTaskReached;
 };
 
 #endif  // CARTESIAN_H

include/FSM/State_MoveJ.h

@@ -13,11 +13,10 @@ public:
     void exit();
     int checkChange(int cmd);
 private:
-    double _costTime;
+    double _speed;
-    HomoMat _goalHomo;
     JointSpaceTraj *_jointTraj;
     bool _reached, _pastReached, _finalReached;
-    std::vector<Vec6> _result;
+    std::vector<Vec6> _qCmd;
 };
 
 #endif  // CARTESIAN_H

include/FSM/State_MoveL.h

@@ -2,7 +2,7 @@
 #define MOVEL_H
 
 #include "FSM/FSMState.h"
-#include "trajectory/CartesionSpaceTraj.h"
+#include "trajectory/EndLineTraj.h"
 
 class State_MoveL : public FSMState{
 public:
@@ -13,22 +13,9 @@ public:
     void exit();
     int checkChange(int cmd);
 private:
-    void quadprogArea();
+    double _speed;
-    double _posSpeed;
+    std::vector<Vec6>  _postures;
-    double _oriSpeed;
+    EndLineTraj *_lineTraj;
-    Vec3 _omega;
-    Vec3 _velocity;
-    Vec6 _twist;
-    Vec6 _pastPosture, _endPosture, _endTwist;
-    HomoMat _endHomoFeedback;
-    Vec6 _Pdes;
-    Vec6 _Pfd;
-    Vec6 _Pkp;
-    Vec6 _endPostureError;
-
-    // 轨迹相关变量
-    std::vector<std::vector<double> >  _posture;
-    CartesionSpaceTraj *_cartesionTraj;
     bool _timeReached, _taskReached, _pastTaskReached, _finalReached;
 };
 #endif  // CARTESIAN_H

include/FSM/State_Passive.h

@@ -1,7 +1,7 @@
 #ifndef PASSIVE_H
 #define PASSIVE_H
 
-#include "FSMState.h"
+#include "FSM/FSMState.h"
 
 class State_Passive : public FSMState{
 public:

include/FSM/State_SaveState.h

@@ -13,9 +13,6 @@ public:
     void exit();
     int checkChange(int cmd);
 private:
-    CSVTool *_csv;
-    Vec6 _q;
-    HomoMat _endHomo;
 };
 
 #endif  // SAVESTATE_H

include/FSM/State_Teach.h

@@ -2,7 +2,6 @@
 #define STATETEACH_H
 
 #include "FSM/FSMState.h"
-#include "common/utilities/CSVTool.h"
 
 class State_Teach : public FSMState{
 public:

include/FSM/State_TeachRepeat.h

@@ -3,7 +3,6 @@
 
 #include "FSM/FSMState.h"
 #include "trajectory/TrajectoryManager.h"
-#include "common/utilities/CSVTool.h"
 
 class State_TeachRepeat : public FSMState{
 public:

include/FSM/State_ToState.h

@@ -19,7 +19,6 @@ private:
     JointSpaceTraj *_jointTraj;
     bool _reach, _pastReach;
     std::string _goalName;
-    // CSVTool *_csv;
 };
 
 #endif  // TOSTATE_H

include/FSM/State_Trajectory.h

@@ -2,7 +2,6 @@
 #define CARTESIANPATH_H
 
 #include "FSM/FSMState.h"
-#include "trajectory/EndHomoTraj.h"
 #include "trajectory/TrajectoryManager.h"
 
 class State_Trajectory : public FSMState{
@@ -16,12 +15,12 @@ public:
     void exit();
     int checkChange(int cmd);
 protected:
-    // EndHomoTraj *_traj;
+    void _setTraj();
-    virtual void _setTraj();
+    void _setTrajSDK();
-    virtual void _setTrajSdk();
     TrajectoryManager *_traj;
     HomoMat _goalHomo;
     Vec6 _goalTwist;
+    double speedTemp;
 
     JointSpaceTraj *_toStartTraj;
     bool _reachedStart = false;
@@ -31,11 +30,8 @@ protected:
     std::vector<StopForTime*> _stopTraj;
     std::vector<EndLineTraj*> _lineTraj;
 
-    long long startTime;
     static std::vector<TrajCmd> _trajCmd;
     Vec6 _posture[2];
-
-    static bool _isTrajFSM;
 };
 
 #endif  // CARTESIANPATH_H

include/common/enumClass.h

@@ -1,13 +1,6 @@
 #ifndef ENUMCLASS_H
 #define ENUMCLASS_H
 
-enum class FrameType{
-    BODY,
-    HIP,
-    ENDEFFECTOR,
-    GLOBAL
-};
-
 enum class FSMMode{
     NORMAL,
     CHANGE
@@ -28,19 +21,7 @@ enum class ArmFSMStateName{
     TEACHREPEAT,
     CALIBRATION,
     SETTRAJ,
-    DANCE00,
-    DANCE01,
-    DANCE02,
-    DANCE03,
-    DANCE04,
-    DANCE05,
-    DANCE06,
-    DANCE07,
-    DANCE08,
-    DANCE09,
     BACKTOSTART,
-    GRIPPER_OPEN,
-    GRIPPER_CLOSE,
     NEXT,
     LOWCMD
 };
@@ -50,20 +31,10 @@ enum class JointMotorType{
     DOUBLE_MOTOR
 };
 
-enum class MotorMountType{
-    STATOR_FIRST,
-    OUTPUT_FIRST
-};
-
-enum class HeadType{
-    TIGER,
-    DOG
-};
-
 enum class Control{
-    _KEYBOARD,
+    KEYBOARD,
-    _SDK,
+    SDK,
-    _JOYSTICK,
+    JOYSTICK
 };
 
 #endif  // ENUMCLASS_H

include/common/math/Filter.h

@@ -1,5 +1,5 @@
-#ifndef LOWPASSFILTER
+#ifndef FILTER
-#define LOWPASSFILTER
+#define FILTER
 
 #include <vector>
 #include <Eigen/Dense>
@@ -26,4 +26,4 @@ private:
     bool _start;
 };
 
-#endif  // LOWPASSFILTER
+#endif  // FILTER

include/common/math/mathTools.h

@@ -2,18 +2,9 @@
 #define MATHTOOLS_H
 
 #include <iostream>
+#include <math.h>
 #include "common/math/mathTypes.h"
 
-// template<typename T1, typename T2>
-// inline T1 max(const T1 a, const T2 b){
-// 	return (a > b ? a : b);
-// }
-
-// template<typename T1, typename T2>
-// inline T1 min(const T1 a, const T2 b){
-// 	return (a < b ? a : b);
-// }
-
 template<typename T>
 T max(T value){
     return value;
@@ -34,21 +25,6 @@ inline T min(const T val0, const Args... restVal){
     return val0 > (min<T>(restVal...)) ? val0 : min<T>(restVal...);
 }
 
-inline Mat2 skew(const double& w){
-    Mat2 mat; mat.setZero();
-    mat(0, 1) = -w;
-    mat(1, 0) =  w;
-    return mat;
-}
-
-inline Mat3 skew(const Vec3& v) {
-    Mat3 m;
-    m << 0, -v(2), v(1),
-            v(2), 0, -v(0),
-            -v(1), v(0), 0;
-    return m;
-}
-
 enum class TurnDirection{
     NOMATTER,
     POSITIVE,
@@ -150,11 +126,9 @@ inline double saturation(const double a, double limValue1, double limValue2){
 
     if(a < lowLim){
         return lowLim;
-    }
+    }else if(a > highLim){
-    else if(a > highLim){
         return highLim;
-    }
+    }else{
-    else{
         return a;
     }
 }
@@ -180,21 +154,17 @@ inline T1 invNormalize(const T0 value, const T1 min, const T2 max, const double
 template<typename T>
 inline T windowFunc(const T x, const T windowRatio, const T xRange=1.0, const T yRange=1.0){
     if((x < 0)||(x > xRange)){
-        // printf("[ERROR] The x of function windowFunc should between [0, xRange]\n");
         std::cout << "[ERROR][windowFunc] The x=" << x << ", which should between [0, xRange]" << std::endl;
     }
     if((windowRatio <= 0)||(windowRatio >= 0.5)){
-        // printf("[ERROR] The windowRatio of function windowFunc should between (0, 0.5)\n");
         std::cout << "[ERROR][windowFunc] The windowRatio=" << windowRatio << ", which should between [0, 0.5]" << std::endl;
     }
 
     if(x/xRange < windowRatio){
         return x * yRange / (xRange * windowRatio);
-    }
+    }else if(x/xRange > 1 - windowRatio){
-    else if(x/xRange > 1 - windowRatio){
         return yRange * (xRange - x)/(xRange * windowRatio);
-    }
+    }else{
-    else{
         return yRange;
     }
 }
@@ -232,212 +202,6 @@ inline void updateAvgCov(T1 &cov, T2 &exp, T3 newValue, double n){
     updateAverage(exp, newValue, n);
 }
 
-/* rotate matrix about x axis */
-inline RotMat rotX(const double &theta) {
-    double s = std::sin(theta);
-    double c = std::cos(theta);
-
-    RotMat R;
-    R << 1, 0, 0, 0, c, -s, 0, s, c;
-    return R;
-}
-
-/* rotate matrix about y axis */
-inline RotMat rotY(const double &theta) {
-    double s = std::sin(theta);
-    double c = std::cos(theta);
-
-    RotMat R;
-    R << c, 0, s, 0, 1, 0, -s, 0, c;
-    return R;
-}
-
-/* rotate matrix about z axis */
-inline RotMat rotZ(const double &theta) {
-    double s = std::sin(theta);
-    double c = std::cos(theta);
-
-    RotMat R;
-    R << c, -s, 0, s, c, 0, 0, 0, 1;
-    return R;
-}
-
-inline RotMat so3ToRotMat(const Vec3& _rot){
-    RotMat R;
-    double theta = _rot.norm();
-    if (fabs(theta) <1e-6)
-        R = RotMat::Identity();
-    else{
-        Vec3  u_axis(_rot/theta);
-        double cos_theta = cos(theta);
-        R = cos_theta*RotMat::Identity()+sin(theta)*skew(u_axis) +(1-cos_theta)*(u_axis*u_axis.transpose());
-    }
-    return R;
-}
-
-/* row pitch yaw to rotate matrix */
-inline RotMat rpyToRotMat(const double& row, const double& pitch, const double& yaw) {
-    RotMat m = rotZ(yaw) * rotY(pitch) * rotX(row);
-    return m;
-}
-
-inline RotMat quatToRotMat(const Quat& q) {
-    double e0 = q(0);
-    double e1 = q(1);
-    double e2 = q(2);
-    double e3 = q(3);
-
-    RotMat R;
-    R << 1 - 2 * (e2 * e2 + e3 * e3), 2 * (e1 * e2 - e0 * e3),
-            2 * (e1 * e3 + e0 * e2), 2 * (e1 * e2 + e0 * e3),
-            1 - 2 * (e1 * e1 + e3 * e3), 2 * (e2 * e3 - e0 * e1),
-            2 * (e1 * e3 - e0 * e2), 2 * (e2 * e3 + e0 * e1),
-            1 - 2 * (e1 * e1 + e2 * e2);
-    return R;
-}
-
-inline Vec3 rotMatToRPY(const Mat3& R) {
-    Vec3 rpy;
-    rpy(0) = atan2(R(2,1),R(2,2));//asin(R(2,1)/cos(rpy(1))); // atan2(R(2,1),R(2,2));
-    rpy(1) = asin(-R(2,0));
-    rpy(2) = atan2(R(1,0),R(0,0));
-    return rpy;
-}
-
-/* rotate matrix to exponential coordinate(omega*theta) */
-inline Vec3 rotMatToExp(const RotMat& rm){
-    double cosValue = rm.trace()/2.0-1/2.0;
-    if(cosValue > 1.0f){
-        cosValue = 1.0f;
-    }else if(cosValue < -1.0f){
-        cosValue = -1.0f;
-    }
-
-    double angle = acos(cosValue);
-    Vec3 exp;
-    if (fabs(angle) < 1e-5){
-        exp=Vec3(0,0,0);
-    }
-    else if (fabs(angle - M_PI) < 1e-5){
-        exp = angle * Vec3(rm(0,0)+1, rm(0,1), rm(0,2)) / sqrt(2*(1+rm(0, 0)));
-    }
-    else{
-        exp=angle/(2.0f*sin(angle))*Vec3(rm(2,1)-rm(1,2),rm(0,2)-rm(2,0),rm(1,0)-rm(0,1));
-    }
-    return exp;
-}
-
-/* add 1 at the end of Vec3 */
-inline Vec4 homoVec(Vec3 v3, double end = 1.0){
-    Vec4 v4;
-    v4.block(0, 0, 3, 1) = v3;
-    v4(3) = end;
-    return v4;
-}
-
-/* remove 1 at the end of Vec4 */
-inline Vec3 noHomoVec(Vec4 v4){
-    Vec3 v3;
-    v3 = v4.block(0, 0, 3, 1);
-    return v3;
-}
-
-/* build Homogeneous Matrix by p and m */
-inline HomoMat homoMatrix(Vec3 p, Mat3 m){
-    HomoMat homoM;
-    homoM.setZero();
-    homoM.topLeftCorner(3, 3) = m;
-    homoM.topRightCorner(3, 1) = p;
-    homoM(3, 3) = 1;
-    return homoM;
-}
-
-/* build Homogeneous Matrix by p and w */
-inline HomoMat homoMatrix(Vec3 p, Vec3 w){
-    HomoMat homoM;
-    homoM.setZero();
-    homoM.topLeftCorner(3, 3) = so3ToRotMat(w);
-    homoM.topRightCorner(3, 1) = p;
-    homoM(3, 3) = 1;
-    return homoM;
-}
-
-/* build Homogeneous Matrix by x axis, y axis and p */
-inline HomoMat homoMatrix(Vec3 x, Vec3 y, Vec3 p){
-    HomoMat homoM;
-    homoM.setZero();
-    Vec3 xN = x.normalized();
-    Vec3 yN = y.normalized();
-    homoM.block(0, 0, 3, 1) = xN;
-    homoM.block(0, 1, 3, 1) = yN;
-    homoM.block(0, 2, 3, 1) = skew(xN) * yN;
-    homoM.topRightCorner(3, 1) = p;
-    homoM(3, 3) = 1;
-    return homoM;
-}
-
-/* build Homogeneous Matrix of rotate joint */
-inline HomoMat homoMatrixRotate(Vec3 q, Vec3 w){
-    HomoMat homoM;
-    Mat3 eye3 = Mat3::Identity();
-    Mat3 rotateM = so3ToRotMat(w);
-    homoM.setZero();
-    homoM.topLeftCorner(3, 3) = rotateM;
-    homoM.topRightCorner(3, 1) = (eye3 - rotateM) * q;
-    // homoM.topRightCorner(3, 1) = q;
-    homoM(3, 3) = 1;
-
-    return homoM;
-}
-
-/* build Homogeneous Matrix by posture */
-inline HomoMat homoMatrixPosture(Vec6 posture){
-    HomoMat homoM;
-    homoM.setZero();
-    homoM.topLeftCorner(3, 3) = rpyToRotMat(posture(0),posture(1),posture(2));
-    homoM.topRightCorner(3, 1) << posture(3),posture(4),posture(5);
-    homoM(3, 3) = 1;
-    return homoM;
-}
-
-/* inverse matrix of homogeneous matrix */
-inline HomoMat homoMatrixInverse(HomoMat homoM){
-    HomoMat homoInv;
-    homoInv.setZero();
-    homoInv.topLeftCorner(3, 3) = homoM.topLeftCorner(3, 3).transpose();
-    homoInv.topRightCorner(3, 1) = -homoM.topLeftCorner(3, 3).transpose() * homoM.topRightCorner(3, 1);
-    homoInv(3, 3) = 1;
-    return homoInv;
-}
-
-/* get position of Homogeneous Matrix */
-inline Vec3 getHomoPosition(HomoMat mat){
-    return mat.block(0, 3, 3, 1);
-}
-
-/* get rotate matrix of Homogeneous Matrix */
-inline RotMat getHomoRotMat(HomoMat mat){
-    return mat.block(0, 0, 3, 3);
-}
-
-/* convert homogeneous matrix to posture vector */
-inline Vec6 homoToPosture(HomoMat mat){
-    Vec6 posture;
-    posture.block(0,0,3,1) = rotMatToRPY(getHomoRotMat(mat));
-    posture.block(3,0,3,1) = getHomoPosition(mat);
-    return posture;
-}
-
-/* convert posture vector matrix to homogeneous */
-inline HomoMat postureToHomo(Vec6 posture){
-    HomoMat homoM;
-    homoM.setZero();
-    homoM.topLeftCorner(3, 3) = rpyToRotMat(posture(0), posture(1), posture(2));
-    homoM.topRightCorner(3, 1) << posture(3), posture(4), posture(5);
-    homoM(3, 3) = 1;
-    return homoM;
-}
-
 // Calculate average value and covariance
 class AvgCov{
 public:

include/common/math/mathTypes.h

@@ -2,6 +2,7 @@
 #define MATHTYPES_H
 
 #include <eigen3/Eigen/Dense>
+#include <vector>
 
 /************************/
 /******** Vector ********/
@@ -96,4 +97,13 @@ inline VecX stdVecToEigenVec(T stdVec){
     VecX eigenVec = Eigen::VectorXd::Map(&stdVec[0], stdVec.size());
     return eigenVec;
 }
+
+inline std::vector<double> EigenVectostdVec(VecX eigenVec){
+    std::vector<double> stdVec;
+    for(int i(0); i<eigenVec.size();i++){
+        stdVec.push_back(eigenVec(i));
+    }
+    return stdVec;
+}
+
 #endif  // MATHTYPES_H

include/common/math/quadProgpp/QuadProg++.hh

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

include/common/math/robotics.h

@@ -3,8 +3,7 @@
 #include <vector>
 #include "common/math/mathTools.h"
 
-namespace mr {
+namespace robo {
-
 /*
  * Function: Find if the value is negligible enough to consider 0
  * Inputs: value to be checked as a double
@@ -12,6 +11,39 @@ namespace mr {
  */
 bool NearZero(const double);
 
+Mat6 rot(const Mat3& E);
+
+Mat6 xlt(const Vec3& r);
+
+/* rotate matrix about x axis */
+RotMat rotX(const double &);
+RotMat rx(const double &xrot);
+
+/* rotate matrix about y axis */
+RotMat rotY(const double &);
+RotMat ry(const double &yrot);
+
+/* rotate matrix about z axis */
+RotMat rotZ(const double &);
+RotMat rz(const double &zrot);
+
+/* row pitch yaw to rotate matrix */
+RotMat rpyToRotMat(const double&, const double&, const double&);
+RotMat rpyToRotMat(const Vec3& rpy);
+
+Vec3 rotMatToRPY(const Mat3& );
+
+RotMat quatToRotMat(const Quat&);
+
+/* convert homogeneous matrix to posture vector */
+Vec6 homoToPosture(HomoMat);
+
+/* convert posture vector matrix to homogeneous */
+HomoMat postureToHomo(Vec6);
+
+RotMat getHomoRotMat(HomoMat T);
+Vec3 getHomoPosition(HomoMat T);
+HomoMat homoMatrix(Vec3 x, Vec3 y, Vec3 p);
 /*
  * Function: Calculate the 6x6 matrix [adV] of the given 6-vector
  * Input: Eigen::VectorXd (6x1)
@@ -131,6 +163,21 @@ Eigen::MatrixXd MatrixExp6(const Eigen::MatrixXd&);
 Eigen::MatrixXd MatrixLog6(const Eigen::MatrixXd&);
 
 
+/*
+ * Functions: Tranforms 3D motion vector form A to B coordinates
+ * Input: T: the cordinate transform form A to B coordiantes for a motion vector
+ * Return : BX_A
+ */
+Mat6 CoordinateTransMotionVector(const HomoMat& T);
+
+/*
+ * Functions: Tranforms 3D force vector form A to B coordinates
+ * Input: T: the cordinate transform form A to B coordiantes for a force vector
+ * Return : {BX_A}*
+ */
+Mat6 CoordinateTransForceVector(const HomoMat& T);
+
+
 /*
  * Function: Compute end effector frame (used for current spatial position calculation)
  * Inputs: Home configuration (position and orientation) of end-effector
@@ -459,72 +506,6 @@ Eigen::VectorXd ForwardDynamics(const Eigen::VectorXd&, const Eigen::VectorXd&,
                                    const std::vector<Eigen::MatrixXd>&, const Eigen::MatrixXd&);
 
 
-/*
- * Function: Compute the joint angles and velocities at the next timestep using
-    first order Euler integration
- * Inputs:
- *  thetalist[in]: n-vector of joint variables
- *  dthetalist[in]: n-vector of joint rates
- *	ddthetalist: n-vector of joint accelerations
- *  dt: The timestep delta t
- *
- * Outputs:
- *  thetalist[out]: Vector of joint variables after dt from first order Euler integration
- *  dthetalist[out]: Vector of joint rates after dt from first order Euler integration
- */
-void EulerStep(Eigen::VectorXd&, Eigen::VectorXd&, const Eigen::VectorXd&, double);
-
-
-/*
- * Function: Compute the joint forces/torques required to move the serial chain along the given
- *	trajectory using inverse dynamics
- * Inputs:
- *  thetamat: An N x n matrix of robot joint variables (N: no. of trajecoty time step points; n: no. of robot joints
- *  dthetamat: An N x n matrix of robot joint velocities
- *  ddthetamat: An N x n matrix of robot joint accelerations
- *	g: Gravity vector g
- *	Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector (if there are no tip forces
- *			 the user should input a zero matrix)
- *  Mlist: List of link frames {i} relative to {i-1} at the home position
- *  Glist: Spatial inertia matrices Gi of the links
- *  Slist: Screw axes Si of the joints in a space frame, in the format
- *         of a matrix with the screw axes as the columns.
- *
- * Outputs:
- *  taumat: The N x n matrix of joint forces/torques for the specified trajectory, where each of the N rows is the vector
- *			of joint forces/torques at each time step
- */
-Eigen::MatrixXd InverseDynamicsTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&,
-	const Eigen::VectorXd&, const Eigen::MatrixXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,
-	const Eigen::MatrixXd&);
-
-
-/*
- * Function: Compute the motion of a serial chain given an open-loop history of joint forces/torques
- * Inputs:
- *  thetalist: n-vector of initial joint variables
- *  dthetalist: n-vector of initial joint rates
- *  taumat: An N x n matrix of joint forces/torques, where each row is is the joint effort at any time step
- *	g: Gravity vector g
- *	Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector (if there are no tip forces
- *			 the user should input a zero matrix)
- *  Mlist: List of link frames {i} relative to {i-1} at the home position
- *  Glist: Spatial inertia matrices Gi of the links
- *  Slist: Screw axes Si of the joints in a space frame, in the format
- *         of a matrix with the screw axes as the columns.
- *	dt: The timestep between consecutive joint forces/torques
- *	intRes: Integration resolution is the number of times integration (Euler) takes places between each time step.
- *			Must be an integer value greater than or equal to 1
- *
- * Outputs: std::vector of [thetamat, dthetamat]
- *  thetamat: The N x n matrix of joint angles resulting from the specified joint forces/torques
- *  dthetamat: The N x n matrix of joint velocities
- */
-std::vector<Eigen::MatrixXd> ForwardDynamicsTrajectory(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::MatrixXd&,
-	const Eigen::VectorXd&, const Eigen::MatrixXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,
-	const Eigen::MatrixXd&, double, int);
-
-
 /*
  * Function: Compute the joint control torques at a particular time instant
  * Inputs:
@@ -551,135 +532,4 @@ Eigen::VectorXd ComputedTorque(const Eigen::VectorXd&, const Eigen::VectorXd&, c
 	const Eigen::VectorXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,
 	const Eigen::MatrixXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, double, double, double);
 
-
-/*
- * Function: Compute s(t) for a cubic time scaling
- * Inputs:
- *  Tf: Total time of the motion in seconds from rest to rest
- *  t: The current time t satisfying 0 < t < Tf
- *
- * Outputs:
- *  st: The path parameter corresponding to a third-order
- *      polynomial motion that begins and ends at zero velocity
- */
-double CubicTimeScaling(double, double);
-
-
-/*
- * Function: Compute s(t) for a quintic time scaling
- * Inputs:
- *  Tf: Total time of the motion in seconds from rest to rest
- *  t: The current time t satisfying 0 < t < Tf
- *
- * Outputs:
- *  st: The path parameter corresponding to a fifth-order
- *      polynomial motion that begins and ends at zero velocity
- *	    and zero acceleration
- */
-double QuinticTimeScaling(double, double);
-
-
-/*
- * Function: Compute a straight-line trajectory in joint space
- * Inputs:
- *  thetastart: The initial joint variables
- *  thetaend: The final joint variables
- *  Tf: Total time of the motion in seconds from rest to rest
- *	N: The number of points N > 1 (Start and stop) in the discrete
- *     representation of the trajectory
- *  method: The time-scaling method, where 3 indicates cubic (third-
- *          order polynomial) time scaling and 5 indicates quintic
- *          (fifth-order polynomial) time scaling
- *
- * Outputs:
- *  traj: A trajectory as an N x n matrix, where each row is an n-vector
- *        of joint variables at an instant in time. The first row is
- *        thetastart and the Nth row is thetaend . The elapsed time
- *        between each row is Tf / (N - 1)
- */
-Eigen::MatrixXd JointTrajectory(const Eigen::VectorXd&, const Eigen::VectorXd&, double, int, int);
-
-
-/*
- * Function: Compute a trajectory as a list of N SE(3) matrices corresponding to
- *			 the screw motion about a space screw axis
- * Inputs:
- *  Xstart: The initial end-effector configuration
- *  Xend: The final end-effector configuration
- *  Tf: Total time of the motion in seconds from rest to rest
- *	N: The number of points N > 1 (Start and stop) in the discrete
- *     representation of the trajectory
- *  method: The time-scaling method, where 3 indicates cubic (third-
- *          order polynomial) time scaling and 5 indicates quintic
- *          (fifth-order polynomial) time scaling
- *
- * Outputs:
- *  traj: The discretized trajectory as a list of N matrices in SE(3)
- *        separated in time by Tf/(N-1). The first in the list is Xstart
- *        and the Nth is Xend
- */
-std::vector<Eigen::MatrixXd> ScrewTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, double, int, int);
-
-
-/*
- * Function: Compute a trajectory as a list of N SE(3) matrices corresponding to
- *			 the origin of the end-effector frame following a straight line
- * Inputs:
- *  Xstart: The initial end-effector configuration
- *  Xend: The final end-effector configuration
- *  Tf: Total time of the motion in seconds from rest to rest
- *	N: The number of points N > 1 (Start and stop) in the discrete
- *     representation of the trajectory
- *  method: The time-scaling method, where 3 indicates cubic (third-
- *          order polynomial) time scaling and 5 indicates quintic
- *          (fifth-order polynomial) time scaling
- *
- * Outputs:
- *  traj: The discretized trajectory as a list of N matrices in SE(3)
- *        separated in time by Tf/(N-1). The first in the list is Xstart
- *        and the Nth is Xend
- * Notes:
- *	This function is similar to ScrewTrajectory, except the origin of the
- *  end-effector frame follows a straight line, decoupled from the rotational
- *  motion.
- */
-std::vector<Eigen::MatrixXd> CartesianTrajectory(const Eigen::MatrixXd&, const Eigen::MatrixXd&, double, int, int);
-
-
-/*
- * Function: Compute the motion of a serial chain given an open-loop history of joint forces/torques
- * Inputs:
- *  thetalist: n-vector of initial joint variables
- *  dthetalist: n-vector of initial joint rates
- *	g: Gravity vector g
- *	Ftipmat: An N x 6 matrix of spatial forces applied by the end-effector (if there are no tip forces
- *			 the user should input a zero matrix)
- *  Mlist: List of link frames {i} relative to {i-1} at the home position
- *  Glist: Spatial inertia matrices Gi of the links
- *  Slist: Screw axes Si of the joints in a space frame, in the format
- *         of a matrix with the screw axes as the columns.
- *  thetamatd: An Nxn matrix of desired joint variables from the reference trajectory
- *  dthetamatd: An Nxn matrix of desired joint velocities
- *  ddthetamatd: An Nxn matrix of desired joint accelerations
- *	gtilde: The gravity vector based on the model of the actual robot (actual values given above)
- *  Mtildelist: The link frame locations based on the model of the actual robot (actual values given above)
- *  Gtildelist: The link spatial inertias based on the model of the actual robot (actual values given above)
- *	Kp: The feedback proportional gain (identical for each joint)
- *	Ki: The feedback integral gain (identical for each joint)
- *	Kd: The feedback derivative gain (identical for each joint)
- *	dt: The timestep between points on the reference trajectory
- *	intRes: Integration resolution is the number of times integration (Euler) takes places between each time step.
- *			Must be an integer value greater than or equal to 1
- *
- * Outputs: std::vector of [taumat, thetamat]
- *  taumat: An Nxn matrix of the controllers commanded joint forces/ torques, where each row of n forces/torques
- *			  corresponds to a single time instant
- *  thetamat: The N x n matrix of actual joint angles
- */
-std::vector<Eigen::MatrixXd> SimulateControl(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&,
-	const Eigen::MatrixXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,
-	const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&,
-	const Eigen::VectorXd&, const std::vector<Eigen::MatrixXd>&, const std::vector<Eigen::MatrixXd>&,
-	double, double, double, double, int);
-
 }

include/common/utilities/CSVTool.h

@@ -181,7 +181,7 @@ inline void CSVTool::readFile(){
 
 inline bool CSVTool::getLine(std::string label, std::vector<double> &values){
     if(_labels.count(label) == 0){
-        // std::cout << "[ERROR] No such label: " << label << std::endl;
+        std::cout << "[ERROR] No such label: " << label << std::endl;
         return false;
     }else{
         _lines.at(_labels[label])->getValues(values);

include/common/utilities/loop.h

@@ -0,0 +1,55 @@
+#ifndef _UNITREE_ARM_LOOP_H_
+#define _UNITREE_ARM_LOOP_H_
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <thread>
+#include <pthread.h>
+#include <vector>
+#include <boost/shared_ptr.hpp>
+#include <boost/function.hpp>
+#include <boost/bind.hpp>
+#include "common/utilities/timer.h"
+
+
+// constexpr int THREAD_PRIORITY    = 99;   // real-time priority
+
+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:
+  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)(); }
+private:
+  boost::function<void ()>  _fp;
+};
+
+#endif  // _UNITREE_ARM_LOOP_H_

include/common/utilities/timer.h

@@ -0,0 +1,56 @@
+#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>
+
+//时间戳  微秒级， 需要#include <sys/time.h> 
+inline long long getSystemTime(){
+    struct timeval t;  
+    gettimeofday(&t, NULL);
+    return 1000000 * t.tv_sec + t.tv_usec;  
+}
+//时间戳  秒级， 需要getSystemTime()
+inline double getTimeSecond(){
+    double time = getSystemTime() * 0.000001;
+    return time;
+}
+/*
+等待函数，微秒级，从startTime开始等待waitTime微秒
+目前不推荐使用，建议使用AbsoluteTimer类
+*/
+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

include/control/CtrlComponents.h

@@ -1,47 +1,49 @@
 #ifndef CTRLCOMPONENTS_H
 #define CTRLCOMPONENTS_H
 
-#include "message/LowlevelCmd.h"
-#include "message/LowlevelState.h"
-#include "interface/IOInterface.h"
-#include "interface/IOROS.h"
-#include "model/ArmDynKineModel.h"
-#include "common/utilities/CSVTool.h"
 #include <string>
 #include <iostream>
-#include "common/math/robotics.h"
+#include "common/utilities/loop.h"
+#include "message/arm_common.h"
+#include "message/LowlevelCmd.h"
+#include "message/LowlevelState.h"
+#include "common/utilities/CSVTool.h"
+#include "model/ArmModel.h"
+#include "interface/IOUDP.h"
+#include "interface/IOROS.h"
+#include "control/armSDK.h"
+
+using namespace std;
 
 struct CtrlComponents{
 public:
     CtrlComponents();
     ~CtrlComponents();
 
-    int dof;
     std::string armConfigPath;
-    LowlevelCmd *lowCmd;
+    CmdPanel *cmdPanel;
-    LowlevelState *lowState;
     IOInterface *ioInter;
-    ArmDynKineModel *armModel;
+    ArmModel *armModel;
     CSVTool *stateCSV;
 
-    //config
+    SendCmd sendCmd; // cmd that receive from SDK 
-    Control ctrl;
+    RecvState recvState;// state that send to SDK
-    bool _hasGripper;
-    std::string ctrl_IP;
     
+    //config
     double dt;
     bool *running;
+    Control ctrl;
+    bool hasGripper;
+    bool isCollisionOpen;
+    double collisionTLimit;
+    bool isPlot;
 
-    void sendRecv();
     void geneObj();
-
-    bool isRecord;
     void writeData();
 private:
-    Vec3 _endPosLocal;
+    std::string ctrl_IP;
-    double _endEffectorMass;
+    uint ctrl_port;
-    Vec3 _endEffectorCom;
+    double _loadWeight;
-    Mat3 _endEffectorInertia;
 };
 
 #endif  // CTRLCOMPONENTS_H

include/control/armSDK.h

@@ -0,0 +1,24 @@
+#ifndef _SDK_H
+#define _SDK_H
+
+#include "message/udp.h"
+#include "control/cmdPanel.h"
+
+class ARMSDK : public CmdPanel{
+public:
+    ARMSDK(std::vector<KeyAction*> events, 
+        EmptyAction emptyAction, const char* IP, uint port, double dt = 0.002);
+    ~ARMSDK();
+    SendCmd getSendCmd();
+    int getState(size_t channelID = 0);
+    void setRecvState(RecvState& recvState);
+private:
+    void _sendRecv();
+    void _read(){};
+    UDPPort *_udp;
+    SendCmd _sendCmd, _sendCmdTemp;
+    RecvState _recvState;
+    size_t _recvLength;
+};
+
+#endif

include/control/cmdPanel.h

@@ -0,0 +1,144 @@
+#ifndef _CMDPANEL_H_
+#define _CMDPANEL_H_
+
+#include <vector>
+#include <deque>
+#include "common/utilities/loop.h"
+#include "message/arm_common.h"
+
+
+enum class KeyPress{
+    RELEASE,
+    PRESS,
+    REPEAT
+};
+
+
+enum class ActionType{
+    EMPTY,
+    STATE,
+    VALUE
+};
+
+
+struct KeyCmd{
+    std::string c;
+    KeyPress keyPress;
+};
+
+
+class KeyAction{
+public:
+    KeyAction(ActionType type);
+    virtual ~KeyAction(){};
+    ActionType getType(){return _type;}
+protected:
+    ActionType _type;
+};
+
+
+class StateAction : public KeyAction{
+public:
+    StateAction(std::string c, int state, KeyPress press = KeyPress::PRESS);
+    virtual ~StateAction(){};
+    int getState(){return _state;};
+    bool handleCmd(KeyCmd keyCmd, int &state);
+protected:
+    int _state;
+    KeyCmd _keyCmdSet;
+};
+
+class EmptyAction : public StateAction{
+public:
+    EmptyAction(int state);
+    ~EmptyAction(){};
+private:
+
+};
+
+
+/* 
+必须为长按, deltaValue为每秒改变量 
+valueAction允许共用按键，例如空格停止
+但是正反转、停止键不可重复
+*/
+class ValueAction : public KeyAction{
+public:    
+    ValueAction(std::string cUp, std::string cDown, double deltaValue, double initValue = 0.0);
+    ValueAction(std::string cUp, std::string cDown, std::string cGoZero, double deltaValue, double initValue = 0.0);
+    ValueAction(std::string cUp, std::string cDown, double deltaValue, double limit1, double limit2, double initValue = 0.0);
+    ValueAction(std::string cUp, std::string cDown, std::string cGoZero, double deltaValue, double limit1, double limit2, double initValue = 0.0);
+
+    ~ValueAction(){};
+    bool handleCmd(KeyCmd keyCmd);
+    void setDt(double dt);
+    double getValue();
+    double getDValue();
+    double getDirection(){return _changeDirection;};
+    void setValue(double value){_value = value;}
+private:
+    double _value;
+    double _changeDirection;
+    double _dV = 0.0; //delta value per delta time
+    double _dt = 0.0;
+    double _dVdt = 0.0; // delta value per second
+    double _lim1, _lim2;
+    bool _hasLim = false;
+    bool _hasGoZero = false;
+
+    KeyCmd _upCmdSet;
+    KeyCmd _downCmdSet;
+    KeyCmd _goZeroCmdSet;
+
+};
+
+
+class CmdPanel{
+public:
+    CmdPanel(std::vector<KeyAction*> events, 
+        EmptyAction emptyAction, size_t channelNum = 1, double dt = 0.002);
+    virtual ~CmdPanel();
+    virtual int getState(size_t channelID = 0);
+    std::vector<double> getValues() {return _values;};
+    std::vector<double> getDValues() {return _dValues;};
+    std::vector<double> getDirections() {return _changeDirections;};
+    void setValue(std::vector<double> values);
+    void setValue(double value, size_t id);
+    virtual std::string getString(std::string slogan);
+    virtual std::vector<double> stringToArray(std::string slogan);
+    virtual std::vector<std::vector<double> > stringToMatrix(std::string slogan);
+    virtual SendCmd getSendCmd();
+    virtual void setRecvState(RecvState& recvState){};
+protected:
+    virtual void _read() = 0;
+    void _run();
+    void _updateState();
+    void _pressKeyboard();
+    void _releaseKeyboard();
+
+    LoopFunc *_runThread;
+    LoopFunc *_readThread;
+
+    std::vector<StateAction> _stateEvents;
+    std::vector<ValueAction> _valueEvents;
+
+    EmptyAction _emptyAction;
+    size_t _actionNum = 0;
+    size_t _stateNum = 0;
+    size_t _valueNum = 0;
+    size_t _channelNum;
+    std::vector<double> _values;
+    std::vector<double> _dValues;
+    std::vector<double> _changeDirections;
+    int _state;
+    std::vector<std::deque<int>> _stateQueue;
+    std::vector<int> _outputState;
+    std::vector<bool> _getState;
+    double _dt;
+    KeyCmd _keyCmd;
+    std::string _cPast = "";
+
+    bool _running = true;
+};
+
+#endif

include/control/joystick.h

@@ -0,0 +1,107 @@
+#ifndef _UNITREE_ARM_JOYSTICK_H_
+#define _UNITREE_ARM_JOYSTICK_H_
+
+#include <vector>
+#include "message/udp.h"
+#include "control/cmdPanel.h"
+#include "message/joystick_common.h"
+#include "message/aliengo_common.h"
+#include "message/b1_common.h"
+#include <math.h>
+
+using namespace UNITREE_LEGGED_SDK_ALIENGO;
+// using namespace UNITREE_LEGGED_SDK_B1;
+
+class UnitreeJoystick : public CmdPanel{
+public:
+    UnitreeJoystick(std::vector<KeyAction*> events, 
+        EmptyAction emptyAction, size_t channelNum = 1,
+        double dt = 0.002)
+        :  CmdPanel(events, emptyAction, channelNum, dt){
+        _udp = new UDPPort("dog", "192.168.123.220", 8082, 8081, HIGH_STATE_LENGTH, BlockYN::NO, 500000);
+
+        _udpCmd = {0};
+        _udpState = {0};
+        _readThread = new LoopFunc("JoyStickRead", 0.0, boost::bind(&UnitreeJoystick::_read, this));
+        _runThread = new LoopFunc("CmdPanelRun", _dt, boost::bind(&UnitreeJoystick::_run, this));
+        _readThread->start();
+        _runThread->start();
+    };
+    ~UnitreeJoystick(){
+        delete _udp;
+        delete _runThread;
+        delete _readThread;
+        
+    };
+private:
+    void _read(){
+        _udp->send((uint8_t*)&_udpCmd, HIGH_CMD_LENGTH);
+        _udp->recv((uint8_t*)&_udpState);
+
+        #ifdef CTRL_BY_ALIENGO_JOYSTICK
+        memcpy(&_keyData, _udpState.wirelessRemote, 40);
+        #else
+        memcpy(&_keyData, &_udpState.wirelessRemote[0], 40);
+        #endif
+
+        _extractCmd();
+        _updateState();
+    };
+    void _extractCmd(){
+        float joyThre = 0.5;    // 手柄数值范围 +-1
+
+        if(((int)_keyData.btn.components.L1 == 1) &&
+        ((int)_keyData.btn.components.L2 == 1)){
+            _keyCmd.c = "l12";
+        }else if((int)_keyData.btn.components.R2  == 1){
+            _keyCmd.c = "r2";
+            if((int)_keyData.btn.components.X  == 1){
+                _keyCmd.c = "r2x";
+            }
+        }else if((int)_keyData.btn.components.R1  == 1){
+            _keyCmd.c = "r1";
+        }else if(fabs(_keyData.lx) > joyThre){
+            _keyData.lx > joyThre?_keyCmd.c = "left_left":_keyCmd.c = "left_right";
+        }else if(fabs(_keyData.ly) > joyThre){
+            _keyData.ly > joyThre?_keyCmd.c = "left_up":_keyCmd.c = "left_down";
+        }else if((int)_keyData.btn.components.up  == 1){
+            _keyCmd.c = "up";
+        }else if((int)_keyData.btn.components.down  == 1){
+            _keyCmd.c = "down";
+        }else if(fabs(_keyData.rx) > joyThre){
+            _keyData.rx > joyThre?_keyCmd.c = "right_left":_keyCmd.c = "right_right";
+        }else if(fabs(_keyData.ry) > joyThre){
+            _keyData.ry > joyThre?_keyCmd.c = "right_up":_keyCmd.c = "right_down";
+        }else if((int)_keyData.btn.components.Y  == 1){
+            _keyCmd.c = "Y";
+        }else if((int)_keyData.btn.components.A  == 1){
+            _keyCmd.c = "A";
+        }else if((int)_keyData.btn.components.X  == 1){
+            _keyCmd.c = "X";
+        }else if((int)_keyData.btn.components.B  == 1){
+            _keyCmd.c = "B";
+        }else if((int)_keyData.btn.components.right  == 1){
+            _keyCmd.c = "left";
+        }else if((int)_keyData.btn.components.left  == 1){
+            _keyCmd.c = "right";
+        }else if((int)_keyData.btn.components.up  == 1){
+            _keyCmd.c = "up";
+        }else if((int)_keyData.btn.components.down  == 1){
+            _keyCmd.c = "down";
+        }else if((int)_keyData.btn.components.select  == 1){
+            _keyCmd.c = "select";
+        }else{
+            _releaseKeyboard();
+            return;
+        }
+
+        _pressKeyboard();
+    };
+
+    xRockerBtnDataStruct _keyData;
+    UDPPort *_udp;
+    HighCmd _udpCmd;
+    HighState _udpState;
+};
+
+#endif  // _UNITREE_ARM_JOYSTICK_H_

include/control/keyboard.h

@@ -0,0 +1,34 @@
+#ifndef _UNITREE_ARM_KEYBOARD_H_
+#define _UNITREE_ARM_KEYBOARD_H_
+
+#include <string>
+#include <vector>
+#include <deque>
+
+#include "message/udp.h"
+#include "common/enumClass.h"
+#include "control/cmdPanel.h"
+
+class Keyboard : public CmdPanel{
+public:
+    Keyboard(std::vector<KeyAction*> events, 
+        EmptyAction emptyAction, size_t channelNum = 1, double dt = 0.002);
+    ~Keyboard();
+    std::string getString(std::string slogan);
+    std::vector<double> stringToArray(std::string slogan);
+    std::vector<std::vector<double> > stringToMatrix(std::string slogan);
+private:
+    void _read();
+    void _pauseKey();
+    void _startKey();
+    void _extractCmd();
+
+    fd_set _set;
+    char _c = '\0';
+
+    termios _oldSettings;
+    termios _newSettings;
+    timeval _tv;
+};
+
+#endif // _UNITREE_ARM_KEYBOARD_H_

include/interface/IOInterface.h

@@ -1,34 +1,29 @@
 #ifndef IOINTERFACE_H
 #define IOINTERFACE_H
 
+#include <string>
 #include "message/LowlevelCmd.h"
 #include "message/LowlevelState.h"
-#include "unitree_arm_sdk/cmdPanel.h"
+#include "control/keyboard.h"
-#include "unitree_arm_sdk/keyboard.h"
+#include "control/joystick.h"
-#include <string>
 #include "common/math/robotics.h"
 
 class IOInterface{
 public:
-    IOInterface(CmdPanel *cmdPanel):_cmdPanel(cmdPanel){}
+    IOInterface(){}
-
+    ~IOInterface(){
-    virtual ~IOInterface(){delete _cmdPanel;};
+        delete lowCmd;
+        delete lowState;
+    };
     virtual bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state) = 0;
-
-    // void zeroCmdPanel(){_cmdPanel->setZero();}
-    std::string getString(std::string slogan){return _cmdPanel->getString(slogan);}
-    std::vector<double> stringToArray(std::string slogan){return _cmdPanel->stringToArray(slogan);}
-    std::vector<std::vector<double> > stringToMatrix(std::string slogan){return _cmdPanel->stringToMatrix(slogan);}
     virtual bool calibration(){return false;};
-
+    bool checkGripper(){return hasGripper;};
-    SendCmd getSendCmd(){return _cmdPanel->getSendCmd();};
+    LowlevelCmd *lowCmd;
-    void getRecvState(RecvState recvState){ _cmdPanel->getRecvState(recvState);};
+    LowlevelState *lowState;
-
+    virtual bool isDisconnect(){ return false;};
-    bool isDisConnect = false;// udp disconnection
+    bool hasErrorState;
-    bool _isDisConnect[7];// joint(motor) disconnection
 protected:
-    uint16_t _isDisConnectCnt[7];
+    bool hasGripper;
-    CmdPanel *_cmdPanel;
 };
 
 #endif  //IOINTERFACE_H

include/interface/IOROS.h

@@ -5,29 +5,20 @@
 
 #include <ros/ros.h>
 #include "interface/IOInterface.h"
-#include "unitree_arm_sdk/keyboard.h"
+#include "message/MotorCmd.h"
-#include "message/unitree_legged_msgs/MotorCmd.h"
+#include "message/MotorState.h"
-#include "message/unitree_legged_msgs/MotorState.h"
 
 class IOROS : public IOInterface{
 public:
-    IOROS(CmdPanel *cmdPanel, bool hasGripper);
+    IOROS();
     ~IOROS();
     bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state);
 private:
-    bool _hasGripper;
     ros::NodeHandle _nm;
-// #ifdef UNITREE_GRIPPER
     ros::Subscriber _servo_sub[7];
     ros::Publisher _servo_pub[7];
     unitree_legged_msgs::MotorState _joint_state[7];
     unitree_legged_msgs::MotorCmd _joint_cmd[7];
-// #else
-//     ros::Subscriber _servo_sub[6];
-//     ros::Publisher _servo_pub[6];
-//     unitree_legged_msgs::MotorState _joint_state[6];
-//     unitree_legged_msgs::MotorCmd _joint_cmd[6];
-// #endif
     void _sendCmd(const LowlevelCmd *cmd);
     void _recvState(LowlevelState *state);
     void _initRecv();

include/interface/IOUDP.h

@@ -2,26 +2,23 @@
 #define IOUDP_H
 
 #include "interface/IOInterface.h"
-// #include "/usr/include/x86_64-linux-gnu/bits/floatn-common.h"
 
 class IOUDP : public IOInterface{
 public:
-    IOUDP(CmdPanel *cmdPanel, const char* IP, bool hasGripper);
+    IOUDP(const char* IP, uint port);
     ~IOUDP();
-    bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state);
-    // void setGripper(int position, int force);
-    // void getGripper(int &position, int &force);
 
+    bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state);
     bool calibration();
+    bool isDisconnect(){ return _ioPort->isDisConnect;}
 private:
     IOPort *_ioPort;
 
     UDPSendCmd _cmd;
     UDPRecvState _state;
-
+    UDPRecvStateOld _stateOld;
     size_t _motorNum;
     size_t _jointNum;
-
     uint8_t _singleState;
     uint8_t _selfCheck[10];
 };

include/message/LowlevelCmd.h

@@ -7,8 +7,6 @@
 #include <iostream>
 
 struct LowlevelCmd{
-private:
-    size_t _dof = 6;
 public:
     std::vector<double> q;
     std::vector<double> dq;
@@ -16,9 +14,10 @@ public:
     std::vector<double> kp;
     std::vector<double> kd;
 
-    std::vector<std::vector<double>> q_cmd_data;
+    std::vector<std::vector<double>> q_data; 
-    std::vector<std::vector<double>> dq_cmd_data;
+    std::vector<std::vector<double>> dq_data; 
-    std::vector<std::vector<double>> tau_cmd_data;
+    std::vector<std::vector<double>> tauf_data;
+    std::vector<std::vector<double>> tau_data;
 
     LowlevelCmd();
     ~LowlevelCmd(){}
@@ -27,13 +26,14 @@ public:
     void setZeroTau();
     void setZeroKp();
     void setZeroKd();
-    void setControlGain();
-    void setControlGain(std::vector<float> KP, std::vector<float> KW);
-    void setQ(std::vector<double> qInput);
     void setQ(VecX qInput);
     void setQd(VecX qDInput);
     void setTau(VecX tauInput);
+
+    void setControlGain();
+    void setControlGain(std::vector<float> KP, std::vector<float> KW);
     void setPassive();
+
     void setGripperGain();
     void setGripperGain(float KP, float KW);
     void setGripperZeroGain();
@@ -43,10 +43,13 @@ public:
     double getGripperQd();
     void setGripperTau(double tauInput);
     double getGripperTau();
+    
     Vec6 getQ();
     Vec6 getQd();
 
     void resizeGripper();
+private:
+    size_t _dof = 6;
 };
 
 

include/message/LowlevelState.h

@@ -3,55 +3,58 @@
 
 #include <iostream>
 #include <vector>
-#include "common/math/mathTypes.h"
 #include "common/math/mathTools.h"
 #include "common/enumClass.h"
-#include "common/utilities/LPFilter.h"
+#include "common/math/Filter.h"
-#include "message/UserValue.h"
 
 struct LowlevelState{
-private:
-    size_t _dof = 6;
 public:
     LowlevelState(double dt);
     ~LowlevelState();
 
-    // MotorState motorState[12];
     std::vector<double> q;
     std::vector<double> dq;
     std::vector<double> ddq;
     std::vector<double> tau;
 
-    std::vector<float> temperature;
+    std::vector<std::vector<double>> q_data; 
+    std::vector<std::vector<double>> dq_data; 
+    std::vector<std::vector<double>> ddq_data;
+    std::vector<std::vector<double>> tau_data;
+
+    std::vector<int> temperature;
     std::vector<uint8_t> errorstate;
+    std::vector<uint8_t> isMotorConnected;
 
     std::vector<double> qFiltered;
     std::vector<double> dqFiltered;
+    std::vector<double> ddqFiltered;
     std::vector<double> tauFiltered;
 
-    std::vector<std::vector<double>> q_state_data;
-    std::vector<std::vector<double>> dq_state_data;
-    std::vector<std::vector<double>> tau_state_data;
-
-    // ArmFSMStateName userCmd;
-    UserValue userValue;
     LPFilter *qFilter;
     LPFilter *dqFilter;
+    LPFilter *ddqFilter;
     LPFilter *tauFilter;
 
     void resizeGripper(double dt);
     void runFilter();
+    bool checkError();
     Vec6 getQ();
     Vec6 getQd();
     Vec6 getQdd();
     Vec6 getTau();
     Vec6 getQFiltered();
     Vec6 getQdFiltered();
+    Vec6 getQddFiltered();
     Vec6 getTauFiltered();
-    double getGripperQ() {return q.at(q.size()-1);}
+    double getGripperQ();
-    double getGripperQd() {return dq.at(q.size()-1);}
+    double getGripperQd();
-    double getGripperTau() {return tau.at(tau.size()-1);}
+    double getGripperTau();
-    double getGripperTauFiltered() {return tauFiltered.at(tau.size()-1);}
+    double getGripperTauFiltered();
+private:
+    size_t _dof = 6;
+    int temporatureLimit = 80.0;// centigrade
+    std::vector<int> _isMotorConnectedCnt;
 };
 
 #endif  //LOWLEVELSTATE_HPP

include/message/UserValue.h

@@ -1,38 +0,0 @@
-#ifndef USERVALUE_H
-#define USERVALUE_H
-
-#include "common/math/mathTypes.h"
-#include "common/utilities/typeTrans.h"
-#include <vector>
-
-template<typename T>
-std::vector<T> cutVector(std::vector<T> vec, size_t startID, size_t length){
-    std::vector<T> result;
-    result.assign(vec.begin()+startID, vec.begin()+startID+length);
-    return result;
-}
-
-struct UserValue{
-    Vec6 moveAxis;
-    double gripperPos;
-
-    void setData(std::vector<double> rawData){
-        if(rawData.size() != 7){
-            std::cout << "[ERROR] UserValue::setData, the size of rawDate is " << rawData.size() << " but not 7" << std::endl;
-        }
-        gripperPos = rawData.at(6);
-        rawData = cutVector(rawData, 0, 6);
-        moveAxis = typeTrans::getValue(rawData, moveAxis);
-    }
-
-    UserValue(){
-        setZero();
-    }
-    void setZero(){
-        moveAxis.setZero();
-        gripperPos = 0;
-        // gripperTau = 0;
-    }
-};
-
-#endif

include/message/aliengo_common.h

@@ -0,0 +1,168 @@
+#ifndef _UNITREE_ARM_ALIENGO_COMMON_H_
+#define _UNITREE_ARM_ALIENGO_COMMON_H_
+
+#include <stdint.h>
+
+namespace UNITREE_LEGGED_SDK_ALIENGO
+{
+
+	constexpr int HIGHLEVEL = 0x00;
+	constexpr int LOWLEVEL  = 0xff;
+	constexpr double PosStopF = (2.146E+9f);
+	constexpr double VelStopF = (16000.0f);
+
+#pragma pack(1)
+
+	// 12 bytes
+	typedef struct
+	{
+		float x;
+		float y;
+		float z;
+	} Cartesian;
+
+	// 53 bytes
+	typedef struct
+	{
+		float quaternion[4];               // quaternion, normalized, (w,x,y,z)
+		float gyroscope[3];                // angular velocity （unit: rad/s)
+		float accelerometer[3];            // m/(s2)
+		float rpy[3];                      // euler angle（unit: rad)
+		int8_t temperature;
+	} IMU;                                 // when under accelerated motion, the attitude of the robot calculated by IMU will drift.
+
+	// 3 bytes
+	typedef struct
+	{
+		uint8_t r;
+		uint8_t g;
+		uint8_t b;
+	} LED;                                 // foot led brightness: 0~255
+
+	// 38 bytes
+	typedef struct
+	{
+		uint8_t mode;                      // motor working mode 
+		float q;                           // current angle (unit: radian)
+		float dq;                          // current velocity (unit: radian/second)
+		float ddq;                       // current acc (unit: radian/second*second)
+		float tauEst;                    // current estimated output torque (unit: N.m)
+		float q_raw;                       // current angle (unit: radian)
+		float dq_raw;                      // current velocity (unit: radian/second)
+		float ddq_raw;
+		int8_t temperature;                // current temperature (temperature conduction is slow that leads to lag)
+        uint32_t reserve[2];   // in reserve[1] returns the brake state.
+	} MotorState;                          // motor feedback
+
+	// 33 bytes
+	typedef struct
+	{
+		uint8_t mode;                      // desired working mode
+		float q;                           // desired angle (unit: radian) 
+		float dq;                          // desired velocity (unit: radian/second)
+		float tau;                       // desired output torque (unit: N.m)
+		float Kp;                        // desired position stiffness (unit: N.m/rad )
+		float Kd;                        // desired velocity stiffness (unit: N.m/(rad/s) )
+        uint32_t reserve[3]; // in reserve[0] sends the brake cmd.
+	} MotorCmd;                            // motor control
+
+	// 891 bytes
+	typedef struct
+	{
+		uint8_t levelFlag;                 // flag to distinguish high level or low level
+		uint16_t commVersion;
+		uint16_t robotID;
+		uint32_t SN; 
+		uint8_t bandWidth;
+		IMU imu;
+		MotorState motorState[20];
+		int16_t footForce[4];              // force sensors
+		int16_t footForceEst[4];           // force sensors
+		uint32_t tick;                     // reference real-time from motion controller (unit: us)
+		uint8_t wirelessRemote[40];        // wireless commands
+		uint32_t reserve;
+		uint32_t crc;
+	} LowState;                            // low level feedback
+
+	// 730 bytes
+	typedef struct 
+	{
+		uint8_t levelFlag;
+		uint16_t commVersion;
+		uint16_t robotID;
+		uint32_t SN;
+		uint8_t bandWidth;
+		MotorCmd motorCmd[20];
+		LED led[4];
+		uint8_t wirelessRemote[40];
+		uint32_t reserve;
+		uint32_t crc;
+	} LowCmd;                              // low level control
+
+	// 244byte
+	typedef struct
+	{
+        uint8_t levelFlag;
+        uint16_t commVersion;
+        uint16_t robotID;
+        uint32_t SN;
+        uint8_t bandWidth;
+        uint8_t mode;
+        IMU imu;
+        float position[3];                 // (unit: m), from robot own odometry in inertial frame, usually drift
+        float velocity[3];                 // (unit: m/s), forwardSpeed, sideSpeed, updownSpeed in body frame
+        float yawSpeed;                    // (unit: rad/s), rotateSpeed in body frame.
+        Cartesian footPosition2Body[4];    // foot position relative to body
+        Cartesian footSpeed2Body[4];       // foot speed relative to body
+        int16_t footForce[4];
+        uint8_t wirelessRemote[40];
+        uint32_t reserve;
+        uint32_t crc;
+	} HighState;                           // high level feedback
+
+	//113byte
+	typedef struct
+	{
+        uint8_t levelFlag;
+        uint16_t commVersion;
+        uint16_t robotID;
+        uint32_t SN;
+        uint8_t bandWidth;
+        uint8_t mode;                      // 0.idle, default stand | 1.force stand (controlled by dBodyHeight + rpy)
+                                           // 2.target velocity walking (controlled by velocity + yawSpeed)
+                                           // 3.target position walking (controlled by position + rpy[2])
+                                           // 4. path mode walking (reserve for future release)
+                                           // 5. position stand down. |6. position stand up |7. damping mode | 8. recovery mode
+        uint8_t gaitType;                  // 0.trot | 1. trot running  | 2.climb stair
+        uint8_t speedLevel;                // 0. default low speed. 1. medium speed 2. high speed. during walking
+        float dFootRaiseHeight;            // (unit: m), swing foot height adjustment from default swing height.
+        float dBodyHeight;                 // (unit: m), body height adjustment from default body height.
+        float position[2];                 // (unit: m), desired x and y position in inertial frame.
+        float rpy[3];                      // (unit: rad), desired yaw-pitch-roll euler angle, expressed in roll(rpy[0]) pitch(rpy[1]) yaw(rpy[2])
+        float velocity[2];                 // (unit: m/s), forwardSpeed, sideSpeed in body frame.
+        float yawSpeed;                    // (unit: rad/s), rotateSpeed in body frame.
+        LED led[4];
+        uint8_t wirelessRemote[40];
+        uint32_t reserve;
+        uint32_t crc;
+    } HighCmd;                             // high level control         uint8_t mode
+
+#pragma pack()
+
+	typedef struct     
+	{
+		unsigned long long TotalCount;     // total loop count
+		unsigned long long SendCount;      // total send count
+		unsigned long long RecvCount;      // total receive count
+		unsigned long long SendError;      // total send error 
+		unsigned long long FlagError;      // total flag error 
+		unsigned long long RecvCRCError;   // total reveive CRC error	
+		unsigned long long RecvLoseError;  // total lose package count	
+	} UDPState;                            // UDP communication state
+
+	constexpr int HIGH_CMD_LENGTH   = (sizeof(HighCmd));
+	constexpr int HIGH_STATE_LENGTH = (sizeof(HighState));
+	
+}
+
+#endif

include/message/arm_common.h

@@ -0,0 +1,150 @@
+#ifndef _UNITREE_ARM_ARM_COMMON_H_
+#define _UNITREE_ARM_ARM_COMMON_H_
+
+#include <stdint.h>
+
+#pragma pack(1)
+
+// 4 Byte
+enum class ArmFSMState{
+    INVALID,
+    PASSIVE,
+    JOINTCTRL,
+    CARTESIAN,
+    MOVEJ,
+    MOVEL,
+    MOVEC,
+    TRAJECTORY,
+    TOSTATE,
+    SAVESTATE,
+    TEACH,
+    TEACHREPEAT,
+    CALIBRATION,
+    SETTRAJ,
+    BACKTOSTART,
+    NEXT,
+    LOWCMD
+};
+
+// 4 Byte
+enum class ArmFSMValue{
+    INVALID,
+    Q,A,
+    W,S,  
+    E,D,
+    R,F,
+    T,G, 
+    Y,H,   
+    DOWN,
+    UP
+};
+
+enum class TrajType{
+    MoveJ,
+    MoveL,
+    MoveC,
+    Stop
+};
+
+// 20 Byte
+struct JointCmd{
+    float T;
+    float W;
+    float Pos;
+    float K_P;
+    float K_W;
+};
+
+typedef struct{
+    uint8_t reserved : 6 ;
+    uint8_t state    : 2 ;//whether motor is connected; 0-ok, 1-disconnected, 2-CRC error
+}Motor_Connected;
+
+typedef struct{
+    int8_t temperature;
+    /* 0x01: phase current is too large
+     * 0x02: phase leakage
+     * 0x04: overheat(including the motor windings and the motor shell)
+     * 0x20: jumped
+     */
+    uint8_t error;
+    Motor_Connected isConnected;
+}Motor_State;
+
+struct JointStateOld{//no error state return
+    float T;
+    float W;
+    float Acc;
+    float Pos;
+};
+
+struct JointState{
+    float T;
+    float W;
+    float Acc;
+    float Pos;
+    Motor_State state[2];
+};
+
+//140bytes
+union UDPSendCmd{
+    uint8_t checkCmd;
+    JointCmd jointCmd[7];
+};
+
+struct UDPRecvStateOld{
+    JointStateOld jointStateOld[7];
+};
+
+struct UDPRecvState{
+    JointState jointState[7];
+};
+
+struct Posture{
+    double roll;
+    double pitch;
+    double yaw;
+    double x;
+    double y;
+    double z;
+};
+
+struct TrajCmd{
+    TrajType trajType;
+    Posture posture[2];
+    double gripperPos;
+    double maxSpeed;
+    double stopTime;
+    int trajOrder;
+};
+
+union ValueUnion{
+    char name[10];
+    JointCmd jointCmd[7];
+    TrajCmd trajCmd;
+};
+
+struct SendCmd{
+    uint8_t head[2];
+    ArmFSMState state;
+    bool track;// whether let arm track jointCmd in State_JOINTCTRL or posture[0] in State_CARTESIAN
+    ValueUnion valueUnion;
+};
+
+
+struct RecvState{
+    uint8_t head[2];
+    ArmFSMState state;
+    JointState jointState[7];
+    Posture cartesianState;
+};
+
+constexpr int SENDCMD_LENGTH    = (sizeof(SendCmd));
+constexpr int RECVSTATE_LENGTH  = (sizeof(RecvState));
+constexpr int JointCmd_LENGTH   = (sizeof(JointCmd));
+constexpr int JointState_LENGTH = (sizeof(JointState));
+constexpr int JointStateOld_LENGTH = (sizeof(JointStateOld));
+
+#pragma pack()
+
+#endif  // _UNITREE_ARM_ARM_MSG_H_

include/message/arm_motor_common.h

@@ -0,0 +1,214 @@
+#ifndef _UNITREE_ARM_ARM_MOTOR_COMMON_H_
+#define _UNITREE_ARM_ARM_MOTOR_COMMON_H_
+
+#include <stdint.h>
+typedef int16_t q15_t;
+
+#pragma pack(1)
+
+// 发送用单个数据数据结构
+typedef union{
+        int32_t           L;
+        uint8_t       u8[4];
+       uint16_t      u16[2];
+       uint32_t         u32;
+          float           F;
+}COMData32;
+
+typedef struct {
+	// 定义 数据包头
+    unsigned char  start[2];     // 包头
+	unsigned char  motorID;      // 电机ID  0,1,2,3 ...   0xBB 表示向所有电机广播（此时无返回）
+	unsigned char  reserved;
+}COMHead;
+
+#pragma pack()
+
+#pragma pack(1)
+
+typedef struct { 
+	
+	   uint8_t  fan_d;       // 关节上的散热风扇转速
+	   uint8_t  Fmusic;      // 电机发声频率   /64*1000   15.625f 频率分度
+	   uint8_t  Hmusic;      // 电机发声强度   推荐值4  声音强度 0.1 分度  
+	   uint8_t  reserved4;
+	
+	   uint8_t  FRGB[4];     // 足端LED 
+	
+}LowHzMotorCmd;
+
+typedef struct {  // 以 4个字节一组排列 ，不然编译器会凑整
+	// 定义 数据
+    uint8_t  mode;        // 关节模式选择
+    uint8_t  ModifyBit;   // 电机控制参数修改位
+    uint8_t  ReadBit;     // 电机控制参数发送位
+    uint8_t  reserved;
+
+    COMData32  Modify;     // 电机参数修改 的数据 
+    //实际给FOC的指令力矩为：
+    //K_P*delta_Pos + K_W*delta_W + T
+    q15_t     T;      // 期望关节的输出力矩（电机本身的力矩）x256, 7 + 8 描述
+    q15_t     W;      // 期望关节速度 （电机本身的速度） x128,       8 + 7描述	
+    int32_t   Pos;      // 期望关节位置 x 16384/6.2832, 14位编码器（主控0点修正，电机关节还是以编码器0点为准）
+
+    q15_t    K_P;      // 关节刚度系数 x2048  4+11 描述
+    q15_t    K_W;      // 关节速度系数 x1024  5+10 描述
+
+    uint8_t LowHzMotorCmdIndex;     // 电机低频率控制命令的索引, 0-7, 分别代表LowHzMotorCmd中的8个字节
+    uint8_t LowHzMotorCmdByte;      // 电机低频率控制命令的字节
+	
+     COMData32  Res[1];    // 通讯 保留字节  用于实现别的一些通讯内容
+	
+}MasterComdV3;   // 加上数据包的包头 和CRC 34字节
+
+typedef struct {
+	// 定义 电机控制命令数据包	
+    COMHead head;    
+    MasterComdV3 Mdata;
+    COMData32 CRCdata;
+}MasterComdDataV3;//返回数据
+
+// typedef struct {
+// 	// 定义 总得485 数据包
+	
+//   MasterComdData M1;
+// 	MasterComdData M2;
+// 	MasterComdData M3;
+	
+// }DMA485TxDataV3;
+
+#pragma pack()
+
+#pragma pack(1)
+
+typedef struct {  // 以 4个字节一组排列 ，不然编译器会凑整
+    // 定义 数据
+    uint8_t  mode;        // 当前关节模式
+    uint8_t  ReadBit;     // 电机控制参数修改     是否成功位
+    int8_t  Temp;        // 电机当前平均温度   
+    uint8_t  MError;      // 电机错误 标识
+ 
+    COMData32  Read;     // 读取的当前 电机 的控制数据 
+    int16_t     T;      // 当前实际电机输出力矩       7 + 8 描述
+
+    int16_t     W;      // 当前实际电机速度（高速）   8 + 7 描述
+    float      LW;      // 当前实际电机速度（低速）   
+
+    int16_t     W2;      // 当前实际关节速度（高速）   8 + 7 描述
+    float      LW2;      // 当前实际关节速度（低速）   
+
+    int16_t    Acc;           // 电机转子加速度       15+0 描述  惯量较小
+    int16_t    OutAcc;        // 输出轴加速度         12+3 描述  惯量较大
+		 
+    int32_t   Pos;      // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准）
+    int32_t   Pos2;     // 关节编码器位置(输出编码器)
+
+    int16_t     gyro[3];  // 电机驱动板6轴传感器数据
+    int16_t     acc[3];   
+
+    // 力传感器的数据   
+    int16_t     Fgyro[3];  //  
+    int16_t     Facc[3];
+    int16_t     Fmag[3];
+    uint8_t     Ftemp;     // 8位表示的温度  7位（-28~100度）  1位0.5度分辨率
+    
+    int16_t     Force16;   // 力传感器高16位数据
+    int8_t      Force8;    // 力传感器低8位数据
+		
+    uint8_t     FError;    //  足端传感器错误标识
+		
+    int8_t      Res[1];    // 通讯 保留字节
+	
+}ServoComdV3;  // 加上数据包的包头 和CRC 78字节（4+70+4）
+
+typedef struct {
+    // 定义 电机控制命令数据包	
+    COMHead        head;
+    ServoComdV3      Mdata;
+
+    COMData32    CRCdata;
+
+}ServoComdDataV3;	//发送数据
+
+// typedef struct {
+// 	// 定义 总的485 接受数据包
+	
+//   ServoComdDataV3 M[3];
+//  // uint8_t  nullbyte1;
+	
+// }DMA485RxDataV3;
+
+
+#pragma pack()
+
+//  00 00 00 00 00 
+//  00 00 00 00 00 
+//  00 00 00 00 00 
+//  00 00 00
+// 数据包默认初始化 
+// 主机发送的数据包 
+/*
+                 Tx485Data[_FR][i].head.start[0] = 0xFE ;     Tx485Data[_FR][i].head.start[1] = 0xEE; // 数据包头					 
+				 Tx485Data[_FR][i].Mdata.ModifyBit = 0xFF;    Tx485Data[_FR][i].Mdata.mode = 0;   // 默认不修改数据 和 电机的默认工作模式				
+				 Tx485Data[_FR][i].head.motorID = i;    0                                          // 目标电机标号
+				 Tx485Data[_FR][i].Mdata.T = 0.0f;                           // 默认目标关节输出力矩                      motor1.Extra_Torque = motorRxData[1].Mdata.T*0.390625f;     // N.M  转化为 N.CM   IQ8描述
+				 Tx485Data[_FR][i].Mdata.Pos = 0x7FE95C80;                   // 默认目标关节位置  不启用位置环          14位分辨率 
+				 Tx485Data[_FR][i].Mdata.W = 16000.0f;                       // 默认目标关节速度  不启用速度环          1+8+7描述     motor1.Target_Speed =  motorRxData[1].Mdata.W*0.0078125f;   // 单位 rad/s	       IQ7描述
+				 Tx485Data[_FR][i].Mdata.K_P = (q15_t)(0.6f*(1<<11));        // 默认关节刚度系数   4+11 描述                     motor1.K_Pos = ((float)motorRxData[1].Mdata.K_P)/(1<<11);      // 描述刚度的通讯数据格式  4+11
+				 Tx485Data[_FR][i].Mdata.K_W = (q15_t)(1.0f*(1<<10));        // 默认关节速度系数   5+10 描述                    motor1.K_Speed = ((float)motorRxData[1].Mdata.K_W)/(1<<10);    // 描述阻尼的通讯数据格式  5+10
+*/ 
+
+enum class MotorType{
+    A1Go1,      // 4.8M baudrate, K_W x1024
+    B1          // 6.0M baudrate, K_W x512
+};
+
+struct MOTOR_send{
+	// 定义 发送格式化数据
+    MasterComdDataV3  motor_send_data;  //电机控制数据结构体
+    MotorType motorType = MotorType::A1Go1;
+	int hex_len = 34;                    //发送的16进制命令数组长度, 34
+    // long long send_time;            //发送该命令的时间, 微秒(us)
+    // 待发送的各项数据
+    unsigned short id;              //电机ID，0xBB代表全部电机
+    unsigned short mode;            //0:空闲, 5:开环转动, 10:闭环FOC控制
+    //实际给FOC的指令力矩为：
+    //K_P*delta_Pos + K_W*delta_W + T
+    float T;                        //期望关节的输出力矩（电机本身的力矩）（Nm）
+    float W;                        //期望关节速度（电机本身的速度）(rad/s)
+    float Pos;                      //期望关节位置（rad）
+    float K_P;                      //关节刚度系数
+    float K_W;                      //关节速度系数
+    COMData32 Res;                  // 通讯 保留字节  用于实现别的一些通讯内容
+};
+
+struct MOTOR_recv{
+    // 定义 接收数据
+    ServoComdDataV3 motor_recv_data;     //电机接收数据结构体
+    MotorType motorType = MotorType::A1Go1;
+    int hex_len = 78;                    //接收的16进制命令数组长度, 78
+    // long long resv_time;            //接收该命令的时间, 微秒(us)
+    bool correct = false;                   //接收数据是否完整（true完整，false不完整）
+    //解读得出的电机数据
+    unsigned char motor_id;         //电机ID
+    unsigned char mode;             //0:空闲, 5:开环转动, 10:闭环FOC控制
+    int Temp;                       //温度
+    unsigned char MError;           //错误码
+
+    float T;                        // 当前实际电机输出力矩
+    float W;                        // 当前实际电机速度（高速）
+    float LW;                       // 当前实际电机速度（低速）
+    int Acc;                      // 电机转子加速度
+    float Pos;                      // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准）
+
+    float gyro[3];                  // 电机驱动板6轴传感器数据
+    float acc[3];
+
+    int8_t Res;
+};
+
+
+extern void modify_data(MOTOR_send* motor_s);
+extern bool extract_data(MOTOR_recv* motor_r);
+
+#endif

include/message/b1_common.h

@@ -0,0 +1,211 @@
+#ifndef _UNITREE_ARM_B1_COMMON_H_
+#define _UNITREE_ARM_B1_COMMON_H_
+
+#include <stdint.h>
+#include <array>
+
+namespace UNITREE_LEGGED_SDK_B1
+{
+
+	constexpr int HIGHLEVEL    = 0xee;
+	constexpr int LOWLEVEL     = 0xff;
+	constexpr int TRIGERLEVEL  = 0xf0;
+	constexpr double PosStopF  = (2.146E+9f);
+	constexpr double VelStopF  = (16000.0f);
+	extern const int LOW_CMD_LENGTH;       // shorter than sizeof(LowCmd),   bytes compressed LowCmd length  
+	extern const int LOW_STATE_LENGTH;     // shorter than sizeof(LowState), bytes compressed LowState length
+
+#pragma pack(1)
+
+	typedef struct
+	{
+		uint8_t off;                       // off 0xA5
+		std::array<uint8_t, 3> reserve;
+	} BmsCmd;
+
+	typedef struct
+	{
+		uint8_t version_h;
+		uint8_t version_l;
+		uint8_t bms_status;
+		uint8_t SOC;                       // SOC 0-100%
+		int32_t current;                   // mA
+		uint16_t cycle;
+		std::array<int8_t, 8> BQ_NTC;                  // x1 degrees centigrade
+		std::array<int8_t, 8> MCU_NTC;                 // x1 degrees centigrade
+		std::array<uint16_t, 30>  cell_vol;            // cell voltage mV
+	} BmsState;
+
+	typedef struct
+	{
+		float x;
+		float y;
+		float z;
+	} Cartesian;
+
+	typedef struct
+	{
+		std::array<float, 4> quaternion;               // quaternion, normalized, (w,x,y,z)
+		std::array<float, 3> gyroscope;                // angular velocity （unit: rad/s)    (raw data)
+		std::array<float, 3> accelerometer;            // m/(s2)                             (raw data)
+		std::array<float, 3> rpy;                      // euler angle（unit: rad)
+		int8_t temperature;
+	} IMU;                                 // when under accelerated motion, the attitude of the robot calculated by IMU will drift.
+
+	typedef struct
+	{
+		uint8_t r;
+		uint8_t g;
+		uint8_t b;
+	} LED;                                 // foot led brightness: 0~255
+
+	typedef struct
+	{
+		uint8_t mode;                      // motor working mode 
+		float q;                           // current angle (unit: radian)
+		float dq;                          // current velocity (unit: radian/second)
+		float ddq;                         // current acc (unit: radian/second*second)
+		float tauEst;                      // current estimated output torque (unit: N.m)
+		float q_raw;                       // current angle (unit: radian)
+		float dq_raw;                      // current velocity (unit: radian/second)
+		float ddq_raw;
+		int8_t temperature;                // current temperature (temperature conduction is slow that leads to lag)
+		std::array<uint32_t, 2> reserve;
+	} MotorState;                          // motor feedback
+
+	typedef struct
+	{
+		uint8_t mode;                      // desired working mode
+		float q;                           // desired angle (unit: radian) 
+		float dq;                          // desired velocity (unit: radian/second)
+		float tau;                         // desired output torque (unit: N.m)
+		float Kp;                          // desired position stiffness (unit: N.m/rad )
+		float Kd;                          // desired velocity stiffness (unit: N.m/(rad/s) )
+		std::array<uint32_t, 3> reserve;
+	} MotorCmd;                            // motor control
+
+	typedef struct
+	{
+		std::array<uint8_t, 2> head;
+		uint8_t levelFlag;
+		uint8_t frameReserve;
+		
+		std::array<uint32_t, 2> SN;
+		std::array<uint32_t, 2> version;
+		uint16_t bandWidth;
+		IMU imu;
+		std::array<MotorState, 20> motorState;
+		BmsState bms;
+		std::array<int16_t, 4> footForce;           // force sensors
+		std::array<int16_t, 4> footForceEst;        // force sensors
+		uint32_t tick;                              // reference real-time from motion controller (unit: us)
+		std::array<uint8_t, 40> wirelessRemote;     // wireless commands
+		uint32_t reserve;
+		
+		uint32_t crc;
+	} LowState;                            // low level feedback
+
+	typedef struct
+	{
+		std::array<uint8_t, 2> head;
+		uint8_t levelFlag;
+		uint8_t frameReserve;
+		
+		std::array<uint32_t, 2> SN;
+		std::array<uint32_t, 2> version;
+		uint16_t bandWidth;
+		std::array<MotorCmd, 20> motorCmd;
+		BmsCmd bms;
+		std::array<uint8_t, 40> wirelessRemote;
+		uint32_t reserve;
+		
+		uint32_t crc;
+	} LowCmd;                              // low level control
+
+	typedef struct
+	{
+		std::array<uint8_t, 2> head;
+		uint8_t levelFlag;
+		uint8_t frameReserve;
+		
+		std::array<uint32_t, 2> SN;
+		std::array<uint32_t, 2> version;
+		uint16_t bandWidth;
+		IMU imu;
+		std::array<MotorState, 20> motorState;
+		BmsState bms;
+		std::array<int16_t, 4> footForce;
+		std::array<int16_t, 4> footForceEst;
+		uint8_t mode;
+		float progress;
+		uint8_t gaitType;                            // 0.idle  1.trot  2.trot running  3.climb stair  4.trot obstacle
+		float footRaiseHeight;                       // (unit: m, default: 0.08m), foot up height while walking
+		std::array<float, 3> position;               // (unit: m), from own odometry in inertial frame, usually drift
+		float bodyHeight;                            // (unit: m, default: 0.28m),
+		std::array<float, 3> velocity;               // (unit: m/s), forwardSpeed, sideSpeed, rotateSpeed in body frame
+		float yawSpeed;                              // (unit: rad/s), rotateSpeed in body frame        
+		std::array<float, 4> rangeObstacle;
+		std::array<Cartesian, 4> footPosition2Body;  // foot position relative to body
+		std::array<Cartesian, 4> footSpeed2Body;     // foot speed relative to body
+		std::array<uint8_t, 40> wirelessRemote;
+		uint32_t reserve;
+		
+		uint32_t crc;
+	} HighState;                           // high level feedback
+
+	typedef struct
+	{
+		std::array<uint8_t, 2> head;
+		uint8_t levelFlag;
+		uint8_t frameReserve;
+		
+		std::array<uint32_t, 2> SN;
+		std::array<uint32_t, 2> version;
+		uint16_t bandWidth;
+		uint8_t mode;                       // 0. idle, default stand  1. force stand (controlled by dBodyHeight + ypr)
+											// 2. target velocity walking (controlled by velocity + yawSpeed)
+											// 3. target position walking (controlled by position + ypr[0])
+											// 4. path mode walking (reserve for future release)
+											// 5. position stand down. 
+											// 6. position stand up 
+											// 7. damping mode 
+											// 8. recovery stand
+											// 9. backflip
+											// 10. jumpYaw
+											// 11. straightHand
+											// 12. dance1
+											// 13. dance2
+
+		uint8_t gaitType;                   // 0.idle  1.trot  2.trot running  3.climb stair  4.trot obstacle
+		uint8_t speedLevel;                 // 0. default low speed. 1. medium speed 2. high speed. during walking, only respond MODE 3
+		float footRaiseHeight;              // (unit: m, default: 0.08m), foot up height while walking, delta value
+		float bodyHeight;                   // (unit: m, default: 0.28m), delta value
+		std::array<float, 2> position;       // (unit: m), desired position in inertial frame
+		std::array<float, 3> euler;         // (unit: rad), roll pitch yaw in stand mode
+		std::array<float, 2> velocity;      // (unit: m/s), forwardSpeed, sideSpeed in body frame
+		float yawSpeed;                     // (unit: rad/s), rotateSpeed in body frame
+		BmsCmd bms;
+		std::array<LED, 4> led;
+		std::array<uint8_t, 40> wirelessRemote;
+		uint32_t reserve;
+		
+		uint32_t crc;
+	} HighCmd;                              // high level control
+
+#pragma pack()
+
+	typedef struct
+	{
+		unsigned long long TotalCount;     // total loop count
+		unsigned long long SendCount;      // total send count
+		unsigned long long RecvCount;      // total receive count
+		unsigned long long SendError;      // total send error 
+		unsigned long long FlagError;      // total flag error 
+		unsigned long long RecvCRCError;   // total reveive CRC error	
+		unsigned long long RecvLoseError;  // total lose package count	
+	} UDPState;                            // UDP communication state
+	constexpr int HIGH_CMD_LENGTH   = (sizeof(HighCmd));
+	constexpr int HIGH_STATE_LENGTH = (sizeof(HighState));
+}
+
+#endif

include/message/joystick_common.h

@@ -0,0 +1,43 @@
+/*****************************************************************
+Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved.
+*****************************************************************/
+#ifndef _UNITREE_ARM_JOYSTICK_COMMON_H_
+#define _UNITREE_ARM_JOYSTICK_COMMON_H_
+#include <stdint.h>
+
+// 16b
+typedef union {
+    struct {
+        uint8_t R1          :1;
+        uint8_t L1          :1;
+        uint8_t start       :1;
+        uint8_t select      :1;
+        uint8_t R2          :1;
+        uint8_t L2          :1;
+        uint8_t F1          :1;
+        uint8_t F2          :1;
+        uint8_t A           :1;
+        uint8_t B           :1;
+        uint8_t X           :1;
+        uint8_t Y           :1;
+        uint8_t up          :1;
+        uint8_t right       :1;
+        uint8_t down        :1;
+        uint8_t left        :1;
+    } components;
+    uint16_t value;
+} xKeySwitchUnion;
+
+// 40 Byte (now used 24B)
+typedef struct {
+    uint8_t head[2];
+    xKeySwitchUnion btn;
+    float lx;
+    float rx;
+    float ry;
+    float L2;
+    float ly;
+    uint8_t idle[16];
+} xRockerBtnDataStruct;
+
+#endif

include/message/udp.h

@@ -0,0 +1,69 @@
+#ifndef _UDP_H
+#define _UDP_H
+
+#include <arpa/inet.h>
+#include <termios.h>
+#include <string>
+#include <string.h>
+#include <vector>
+#include "common/utilities/timer.h"
+#include "message/arm_motor_common.h"
+
+enum class BlockYN{
+    YES,
+    NO
+};
+
+
+class IOPort{
+public:
+    IOPort(std::string name, BlockYN blockYN, size_t recvLength, size_t timeOutUs)
+        :_name(name){
+        resetIO(blockYN, recvLength, timeOutUs);
+    }
+    virtual ~IOPort(){}
+    virtual size_t send(uint8_t *sendMsg, size_t sendLength) = 0;
+    virtual size_t recv(uint8_t *recvMsg, size_t recvLength) = 0;
+    virtual size_t recv(uint8_t *recvMsg) = 0;
+    virtual bool sendRecv(std::vector<MOTOR_send> &sendVec, std::vector<MOTOR_recv> &recvVec) = 0;
+    void resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs);
+    bool isDisConnect = false;
+protected:
+    std::string _name;
+    BlockYN _blockYN = BlockYN::NO;
+    size_t _recvLength;
+    timeval _timeout;
+    timeval _timeoutSaved;
+    uint16_t _isDisConnectCnt;
+};
+
+
+class UDPPort : public IOPort{
+public:
+    UDPPort(std::string name, std::string toIP, uint toPort, uint ownPort, 
+            size_t recvLength = 0,
+            BlockYN blockYN = BlockYN::NO,
+            size_t timeOutUs = 20000);
+    ~UDPPort();
+    size_t send(uint8_t *sendMsg, size_t sendMsgLength);
+    size_t recv(uint8_t *recvMsg, size_t recvLength);
+    size_t recv(uint8_t *recvMsg);
+    bool sendRecv(std::vector<MOTOR_send> &sendVec, std::vector<MOTOR_recv> &recvVec);
+private:
+    size_t _recvBlock(uint8_t *recvMsg, size_t recvLength);
+    size_t _recvUnBlock(uint8_t *recvMsg, size_t recvLength);
+    sockaddr_in _ownAddr, _toAddr, _fromAddr;
+    socklen_t _sockaddrSize;
+    int _sockfd;
+    int _on = 1;
+    size_t _sentLength;
+
+    uint8_t _sendBytes[238];    // 7 motors
+    uint8_t _recvBytes[546];   // 7 motors
+
+    fd_set _rSet;
+};
+
+
+
+#endif

include/message/unitree_legged_msgs/Cartesian.h

@@ -1,215 +0,0 @@
-// Generated by gencpp from file unitree_legged_msgs/Cartesian.msg
-// DO NOT EDIT!
-
-
-#ifndef UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H
-#define UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H
-
-
-#include <string>
-#include <vector>
-#include <map>
-
-#include <ros/types.h>
-#include <ros/serialization.h>
-#include <ros/builtin_message_traits.h>
-#include <ros/message_operations.h>
-
-
-namespace unitree_legged_msgs
-{
-template <class ContainerAllocator>
-struct Cartesian_
-{
-  typedef Cartesian_<ContainerAllocator> Type;
-
-  Cartesian_()
-    : x(0.0)
-    , y(0.0)
-    , z(0.0)  {
-    }
-  Cartesian_(const ContainerAllocator& _alloc)
-    : x(0.0)
-    , y(0.0)
-    , z(0.0)  {
-  (void)_alloc;
-    }
-
-
-
-   typedef float _x_type;
-  _x_type x;
-
-   typedef float _y_type;
-  _y_type y;
-
-   typedef float _z_type;
-  _z_type z;
-
-
-
-
-
-  typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> > Ptr;
-  typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> const> ConstPtr;
-
-}; // struct Cartesian_
-
-typedef ::unitree_legged_msgs::Cartesian_<std::allocator<void> > Cartesian;
-
-typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian > CartesianPtr;
-typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian const> CartesianConstPtr;
-
-// constants requiring out of line definition
-
-
-
-template<typename ContainerAllocator>
-std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::Cartesian_<ContainerAllocator> & v)
-{
-ros::message_operations::Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, "", v);
-return s;
-}
-
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator==(const ::unitree_legged_msgs::Cartesian_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::Cartesian_<ContainerAllocator2> & rhs)
-{
-  return lhs.x == rhs.x &&
-    lhs.y == rhs.y &&
-    lhs.z == rhs.z;
-}
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator!=(const ::unitree_legged_msgs::Cartesian_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::Cartesian_<ContainerAllocator2> & rhs)
-{
-  return !(lhs == rhs);
-}
-
-
-} // namespace unitree_legged_msgs
-
-namespace ros
-{
-namespace message_traits
-{
-
-
-
-
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
-  : FalseType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> const>
-  : FalseType
-  { };
-
-
-template<class ContainerAllocator>
-struct MD5Sum< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "cc153912f1453b708d221682bc23d9ac";
-  }
-
-  static const char* value(const ::unitree_legged_msgs::Cartesian_<ContainerAllocator>&) { return value(); }
-  static const uint64_t static_value1 = 0xcc153912f1453b70ULL;
-  static const uint64_t static_value2 = 0x8d221682bc23d9acULL;
-};
-
-template<class ContainerAllocator>
-struct DataType< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "unitree_legged_msgs/Cartesian";
-  }
-
-  static const char* value(const ::unitree_legged_msgs::Cartesian_<ContainerAllocator>&) { return value(); }
-};
-
-template<class ContainerAllocator>
-struct Definition< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "float32 x\n"
-"float32 y\n"
-"float32 z\n"
-;
-  }
-
-  static const char* value(const ::unitree_legged_msgs::Cartesian_<ContainerAllocator>&) { return value(); }
-};
-
-} // namespace message_traits
-} // namespace ros
-
-namespace ros
-{
-namespace serialization
-{
-
-  template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
-  {
-    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
-    {
-      stream.next(m.x);
-      stream.next(m.y);
-      stream.next(m.z);
-    }
-
-    ROS_DECLARE_ALLINONE_SERIALIZER
-  }; // struct Cartesian_
-
-} // namespace serialization
-} // namespace ros
-
-namespace ros
-{
-namespace message_operations
-{
-
-template<class ContainerAllocator>
-struct Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >
-{
-  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::Cartesian_<ContainerAllocator>& v)
-  {
-    s << indent << "x: ";
-    Printer<float>::stream(s, indent + "  ", v.x);
-    s << indent << "y: ";
-    Printer<float>::stream(s, indent + "  ", v.y);
-    s << indent << "z: ";
-    Printer<float>::stream(s, indent + "  ", v.z);
-  }
-};
-
-} // namespace message_operations
-} // namespace ros
-
-#endif // UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H

include/message/unitree_legged_msgs/HighCmd.h

@@ -1,403 +0,0 @@
-// Generated by gencpp from file unitree_legged_msgs/HighCmd.msg
-// DO NOT EDIT!
-
-
-#ifndef UNITREE_LEGGED_MSGS_MESSAGE_HIGHCMD_H
-#define UNITREE_LEGGED_MSGS_MESSAGE_HIGHCMD_H
-
-
-#include <string>
-#include <vector>
-#include <map>
-
-#include <ros/types.h>
-#include <ros/serialization.h>
-#include <ros/builtin_message_traits.h>
-#include <ros/message_operations.h>
-
-#include <unitree_legged_msgs/LED.h>
-
-namespace unitree_legged_msgs
-{
-template <class ContainerAllocator>
-struct HighCmd_
-{
-  typedef HighCmd_<ContainerAllocator> Type;
-
-  HighCmd_()
-    : levelFlag(0)
-    , commVersion(0)
-    , robotID(0)
-    , SN(0)
-    , bandWidth(0)
-    , mode(0)
-    , forwardSpeed(0.0)
-    , sideSpeed(0.0)
-    , rotateSpeed(0.0)
-    , bodyHeight(0.0)
-    , footRaiseHeight(0.0)
-    , yaw(0.0)
-    , pitch(0.0)
-    , roll(0.0)
-    , led()
-    , wirelessRemote()
-    , AppRemote()
-    , reserve(0)
-    , crc(0)  {
-      wirelessRemote.assign(0);
-
-      AppRemote.assign(0);
-  }
-  HighCmd_(const ContainerAllocator& _alloc)
-    : levelFlag(0)
-    , commVersion(0)
-    , robotID(0)
-    , SN(0)
-    , bandWidth(0)
-    , mode(0)
-    , forwardSpeed(0.0)
-    , sideSpeed(0.0)
-    , rotateSpeed(0.0)
-    , bodyHeight(0.0)
-    , footRaiseHeight(0.0)
-    , yaw(0.0)
-    , pitch(0.0)
-    , roll(0.0)
-    , led()
-    , wirelessRemote()
-    , AppRemote()
-    , reserve(0)
-    , crc(0)  {
-  (void)_alloc;
-      led.assign( ::unitree_legged_msgs::LED_<ContainerAllocator> (_alloc));
-
-      wirelessRemote.assign(0);
-
-      AppRemote.assign(0);
-  }
-
-
-
-   typedef uint8_t _levelFlag_type;
-  _levelFlag_type levelFlag;
-
-   typedef uint16_t _commVersion_type;
-  _commVersion_type commVersion;
-
-   typedef uint16_t _robotID_type;
-  _robotID_type robotID;
-
-   typedef uint32_t _SN_type;
-  _SN_type SN;
-
-   typedef uint8_t _bandWidth_type;
-  _bandWidth_type bandWidth;
-
-   typedef uint8_t _mode_type;
-  _mode_type mode;
-
-   typedef float _forwardSpeed_type;
-  _forwardSpeed_type forwardSpeed;
-
-   typedef float _sideSpeed_type;
-  _sideSpeed_type sideSpeed;
-
-   typedef float _rotateSpeed_type;
-  _rotateSpeed_type rotateSpeed;
-
-   typedef float _bodyHeight_type;
-  _bodyHeight_type bodyHeight;
-
-   typedef float _footRaiseHeight_type;
-  _footRaiseHeight_type footRaiseHeight;
-
-   typedef float _yaw_type;
-  _yaw_type yaw;
-
-   typedef float _pitch_type;
-  _pitch_type pitch;
-
-   typedef float _roll_type;
-  _roll_type roll;
-
-   typedef boost::array< ::unitree_legged_msgs::LED_<ContainerAllocator> , 4>  _led_type;
-  _led_type led;
-
-   typedef boost::array<uint8_t, 40>  _wirelessRemote_type;
-  _wirelessRemote_type wirelessRemote;
-
-   typedef boost::array<uint8_t, 40>  _AppRemote_type;
-  _AppRemote_type AppRemote;
-
-   typedef uint32_t _reserve_type;
-  _reserve_type reserve;
-
-   typedef int32_t _crc_type;
-  _crc_type crc;
-
-
-
-
-
-  typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> > Ptr;
-  typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> const> ConstPtr;
-
-}; // struct HighCmd_
-
-typedef ::unitree_legged_msgs::HighCmd_<std::allocator<void> > HighCmd;
-
-typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd > HighCmdPtr;
-typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd const> HighCmdConstPtr;
-
-// constants requiring out of line definition
-
-
-
-template<typename ContainerAllocator>
-std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::HighCmd_<ContainerAllocator> & v)
-{
-ros::message_operations::Printer< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >::stream(s, "", v);
-return s;
-}
-
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator==(const ::unitree_legged_msgs::HighCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::HighCmd_<ContainerAllocator2> & rhs)
-{
-  return lhs.levelFlag == rhs.levelFlag &&
-    lhs.commVersion == rhs.commVersion &&
-    lhs.robotID == rhs.robotID &&
-    lhs.SN == rhs.SN &&
-    lhs.bandWidth == rhs.bandWidth &&
-    lhs.mode == rhs.mode &&
-    lhs.forwardSpeed == rhs.forwardSpeed &&
-    lhs.sideSpeed == rhs.sideSpeed &&
-    lhs.rotateSpeed == rhs.rotateSpeed &&
-    lhs.bodyHeight == rhs.bodyHeight &&
-    lhs.footRaiseHeight == rhs.footRaiseHeight &&
-    lhs.yaw == rhs.yaw &&
-    lhs.pitch == rhs.pitch &&
-    lhs.roll == rhs.roll &&
-    lhs.led == rhs.led &&
-    lhs.wirelessRemote == rhs.wirelessRemote &&
-    lhs.AppRemote == rhs.AppRemote &&
-    lhs.reserve == rhs.reserve &&
-    lhs.crc == rhs.crc;
-}
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator!=(const ::unitree_legged_msgs::HighCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::HighCmd_<ContainerAllocator2> & rhs)
-{
-  return !(lhs == rhs);
-}
-
-
-} // namespace unitree_legged_msgs
-
-namespace ros
-{
-namespace message_traits
-{
-
-
-
-
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
-  : FalseType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> const>
-  : FalseType
-  { };
-
-
-template<class ContainerAllocator>
-struct MD5Sum< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "1a655499a3f64905db59ceed65ca774a";
-  }
-
-  static const char* value(const ::unitree_legged_msgs::HighCmd_<ContainerAllocator>&) { return value(); }
-  static const uint64_t static_value1 = 0x1a655499a3f64905ULL;
-  static const uint64_t static_value2 = 0xdb59ceed65ca774aULL;
-};
-
-template<class ContainerAllocator>
-struct DataType< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "unitree_legged_msgs/HighCmd";
-  }
-
-  static const char* value(const ::unitree_legged_msgs::HighCmd_<ContainerAllocator>&) { return value(); }
-};
-
-template<class ContainerAllocator>
-struct Definition< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "uint8 levelFlag\n"
-"uint16 commVersion              # Old version Aliengo does not have\n"
-"uint16 robotID                  # Old version Aliengo does not have\n"
-"uint32 SN                       # Old version Aliengo does not have\n"
-"uint8 bandWidth                 # Old version Aliengo does not have\n"
-"uint8 mode\n"
-"float32 forwardSpeed\n"
-"float32 sideSpeed\n"
-"float32 rotateSpeed	\n"
-"float32 bodyHeight\n"
-"float32 footRaiseHeight\n"
-"float32 yaw\n"
-"float32 pitch\n"
-"float32 roll\n"
-"LED[4] led\n"
-"uint8[40] wirelessRemote\n"
-"uint8[40] AppRemote             # Old version Aliengo does not have\n"
-"uint32 reserve                  # Old version Aliengo does not have\n"
-"int32 crc\n"
-"================================================================================\n"
-"MSG: unitree_legged_msgs/LED\n"
-"uint8 r\n"
-"uint8 g\n"
-"uint8 b\n"
-;
-  }
-
-  static const char* value(const ::unitree_legged_msgs::HighCmd_<ContainerAllocator>&) { return value(); }
-};
-
-} // namespace message_traits
-} // namespace ros
-
-namespace ros
-{
-namespace serialization
-{
-
-  template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
-  {
-    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
-    {
-      stream.next(m.levelFlag);
-      stream.next(m.commVersion);
-      stream.next(m.robotID);
-      stream.next(m.SN);
-      stream.next(m.bandWidth);
-      stream.next(m.mode);
-      stream.next(m.forwardSpeed);
-      stream.next(m.sideSpeed);
-      stream.next(m.rotateSpeed);
-      stream.next(m.bodyHeight);
-      stream.next(m.footRaiseHeight);
-      stream.next(m.yaw);
-      stream.next(m.pitch);
-      stream.next(m.roll);
-      stream.next(m.led);
-      stream.next(m.wirelessRemote);
-      stream.next(m.AppRemote);
-      stream.next(m.reserve);
-      stream.next(m.crc);
-    }
-
-    ROS_DECLARE_ALLINONE_SERIALIZER
-  }; // struct HighCmd_
-
-} // namespace serialization
-} // namespace ros
-
-namespace ros
-{
-namespace message_operations
-{
-
-template<class ContainerAllocator>
-struct Printer< ::unitree_legged_msgs::HighCmd_<ContainerAllocator> >
-{
-  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::HighCmd_<ContainerAllocator>& v)
-  {
-    s << indent << "levelFlag: ";
-    Printer<uint8_t>::stream(s, indent + "  ", v.levelFlag);
-    s << indent << "commVersion: ";
-    Printer<uint16_t>::stream(s, indent + "  ", v.commVersion);
-    s << indent << "robotID: ";
-    Printer<uint16_t>::stream(s, indent + "  ", v.robotID);
-    s << indent << "SN: ";
-    Printer<uint32_t>::stream(s, indent + "  ", v.SN);
-    s << indent << "bandWidth: ";
-    Printer<uint8_t>::stream(s, indent + "  ", v.bandWidth);
-    s << indent << "mode: ";
-    Printer<uint8_t>::stream(s, indent + "  ", v.mode);
-    s << indent << "forwardSpeed: ";
-    Printer<float>::stream(s, indent + "  ", v.forwardSpeed);
-    s << indent << "sideSpeed: ";
-    Printer<float>::stream(s, indent + "  ", v.sideSpeed);
-    s << indent << "rotateSpeed: ";
-    Printer<float>::stream(s, indent + "  ", v.rotateSpeed);
-    s << indent << "bodyHeight: ";
-    Printer<float>::stream(s, indent + "  ", v.bodyHeight);
-    s << indent << "footRaiseHeight: ";
-    Printer<float>::stream(s, indent + "  ", v.footRaiseHeight);
-    s << indent << "yaw: ";
-    Printer<float>::stream(s, indent + "  ", v.yaw);
-    s << indent << "pitch: ";
-    Printer<float>::stream(s, indent + "  ", v.pitch);
-    s << indent << "roll: ";
-    Printer<float>::stream(s, indent + "  ", v.roll);
-    s << indent << "led[]" << std::endl;
-    for (size_t i = 0; i < v.led.size(); ++i)
-    {
-      s << indent << "  led[" << i << "]: ";
-      s << std::endl;
-      s << indent;
-      Printer< ::unitree_legged_msgs::LED_<ContainerAllocator> >::stream(s, indent + "    ", v.led[i]);
-    }
-    s << indent << "wirelessRemote[]" << std::endl;
-    for (size_t i = 0; i < v.wirelessRemote.size(); ++i)
-    {
-      s << indent << "  wirelessRemote[" << i << "]: ";
-      Printer<uint8_t>::stream(s, indent + "  ", v.wirelessRemote[i]);
-    }
-    s << indent << "AppRemote[]" << std::endl;
-    for (size_t i = 0; i < v.AppRemote.size(); ++i)
-    {
-      s << indent << "  AppRemote[" << i << "]: ";
-      Printer<uint8_t>::stream(s, indent + "  ", v.AppRemote[i]);
-    }
-    s << indent << "reserve: ";
-    Printer<uint32_t>::stream(s, indent + "  ", v.reserve);
-    s << indent << "crc: ";
-    Printer<int32_t>::stream(s, indent + "  ", v.crc);
-  }
-};
-
-} // namespace message_operations
-} // namespace ros
-
-#endif // UNITREE_LEGGED_MSGS_MESSAGE_HIGHCMD_H

include/message/unitree_legged_msgs/HighState.h

@@ -1,497 +0,0 @@
-// Generated by gencpp from file unitree_legged_msgs/HighState.msg
-// DO NOT EDIT!
-
-
-#ifndef UNITREE_LEGGED_MSGS_MESSAGE_HIGHSTATE_H
-#define UNITREE_LEGGED_MSGS_MESSAGE_HIGHSTATE_H
-
-
-#include <string>
-#include <vector>
-#include <map>
-
-#include <ros/types.h>
-#include <ros/serialization.h>
-#include <ros/builtin_message_traits.h>
-#include <ros/message_operations.h>
-
-#include <unitree_legged_msgs/IMU.h>
-#include <unitree_legged_msgs/Cartesian.h>
-#include <unitree_legged_msgs/Cartesian.h>
-#include <unitree_legged_msgs/Cartesian.h>
-
-namespace unitree_legged_msgs
-{
-template <class ContainerAllocator>
-struct HighState_
-{
-  typedef HighState_<ContainerAllocator> Type;
-
-  HighState_()
-    : levelFlag(0)
-    , commVersion(0)
-    , robotID(0)
-    , SN(0)
-    , bandWidth(0)
-    , mode(0)
-    , imu()
-    , forwardSpeed(0.0)
-    , sideSpeed(0.0)
-    , rotateSpeed(0.0)
-    , bodyHeight(0.0)
-    , updownSpeed(0.0)
-    , forwardPosition(0.0)
-    , sidePosition(0.0)
-    , footPosition2Body()
-    , footSpeed2Body()
-    , footForce()
-    , footForceEst()
-    , tick(0)
-    , wirelessRemote()
-    , reserve(0)
-    , crc(0)
-    , eeForce()
-    , jointP()  {
-      footForce.assign(0);
-
-      footForceEst.assign(0);
-
-      wirelessRemote.assign(0);
-
-      jointP.assign(0.0);
-  }
-  HighState_(const ContainerAllocator& _alloc)
-    : levelFlag(0)
-    , commVersion(0)
-    , robotID(0)
-    , SN(0)
-    , bandWidth(0)
-    , mode(0)
-    , imu(_alloc)
-    , forwardSpeed(0.0)
-    , sideSpeed(0.0)
-    , rotateSpeed(0.0)
-    , bodyHeight(0.0)
-    , updownSpeed(0.0)
-    , forwardPosition(0.0)
-    , sidePosition(0.0)
-    , footPosition2Body()
-    , footSpeed2Body()
-    , footForce()
-    , footForceEst()
-    , tick(0)
-    , wirelessRemote()
-    , reserve(0)
-    , crc(0)
-    , eeForce()
-    , jointP()  {
-  (void)_alloc;
-      footPosition2Body.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
-
-      footSpeed2Body.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
-
-      footForce.assign(0);
-
-      footForceEst.assign(0);
-
-      wirelessRemote.assign(0);
-
-      eeForce.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
-
-      jointP.assign(0.0);
-  }
-
-
-
-   typedef uint8_t _levelFlag_type;
-  _levelFlag_type levelFlag;
-
-   typedef uint16_t _commVersion_type;
-  _commVersion_type commVersion;
-
-   typedef uint16_t _robotID_type;
-  _robotID_type robotID;
-
-   typedef uint32_t _SN_type;
-  _SN_type SN;
-
-   typedef uint8_t _bandWidth_type;
-  _bandWidth_type bandWidth;
-
-   typedef uint8_t _mode_type;
-  _mode_type mode;
-
-   typedef  ::unitree_legged_msgs::IMU_<ContainerAllocator>  _imu_type;
-  _imu_type imu;
-
-   typedef float _forwardSpeed_type;
-  _forwardSpeed_type forwardSpeed;
-
-   typedef float _sideSpeed_type;
-  _sideSpeed_type sideSpeed;
-
-   typedef float _rotateSpeed_type;
-  _rotateSpeed_type rotateSpeed;
-
-   typedef float _bodyHeight_type;
-  _bodyHeight_type bodyHeight;
-
-   typedef float _updownSpeed_type;
-  _updownSpeed_type updownSpeed;
-
-   typedef float _forwardPosition_type;
-  _forwardPosition_type forwardPosition;
-
-   typedef float _sidePosition_type;
-  _sidePosition_type sidePosition;
-
-   typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4>  _footPosition2Body_type;
-  _footPosition2Body_type footPosition2Body;
-
-   typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4>  _footSpeed2Body_type;
-  _footSpeed2Body_type footSpeed2Body;
-
-   typedef boost::array<int16_t, 4>  _footForce_type;
-  _footForce_type footForce;
-
-   typedef boost::array<int16_t, 4>  _footForceEst_type;
-  _footForceEst_type footForceEst;
-
-   typedef uint32_t _tick_type;
-  _tick_type tick;
-
-   typedef boost::array<uint8_t, 40>  _wirelessRemote_type;
-  _wirelessRemote_type wirelessRemote;
-
-   typedef uint32_t _reserve_type;
-  _reserve_type reserve;
-
-   typedef uint32_t _crc_type;
-  _crc_type crc;
-
-   typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4>  _eeForce_type;
-  _eeForce_type eeForce;
-
-   typedef boost::array<float, 12>  _jointP_type;
-  _jointP_type jointP;
-
-
-
-
-
-  typedef boost::shared_ptr< ::unitree_legged_msgs::HighState_<ContainerAllocator> > Ptr;
-  typedef boost::shared_ptr< ::unitree_legged_msgs::HighState_<ContainerAllocator> const> ConstPtr;
-
-}; // struct HighState_
-
-typedef ::unitree_legged_msgs::HighState_<std::allocator<void> > HighState;
-
-typedef boost::shared_ptr< ::unitree_legged_msgs::HighState > HighStatePtr;
-typedef boost::shared_ptr< ::unitree_legged_msgs::HighState const> HighStateConstPtr;
-
-// constants requiring out of line definition
-
-
-
-template<typename ContainerAllocator>
-std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::HighState_<ContainerAllocator> & v)
-{
-ros::message_operations::Printer< ::unitree_legged_msgs::HighState_<ContainerAllocator> >::stream(s, "", v);
-return s;
-}
-
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator==(const ::unitree_legged_msgs::HighState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::HighState_<ContainerAllocator2> & rhs)
-{
-  return lhs.levelFlag == rhs.levelFlag &&
-    lhs.commVersion == rhs.commVersion &&
-    lhs.robotID == rhs.robotID &&
-    lhs.SN == rhs.SN &&
-    lhs.bandWidth == rhs.bandWidth &&
-    lhs.mode == rhs.mode &&
-    lhs.imu == rhs.imu &&
-    lhs.forwardSpeed == rhs.forwardSpeed &&
-    lhs.sideSpeed == rhs.sideSpeed &&
-    lhs.rotateSpeed == rhs.rotateSpeed &&
-    lhs.bodyHeight == rhs.bodyHeight &&
-    lhs.updownSpeed == rhs.updownSpeed &&
-    lhs.forwardPosition == rhs.forwardPosition &&
-    lhs.sidePosition == rhs.sidePosition &&
-    lhs.footPosition2Body == rhs.footPosition2Body &&
-    lhs.footSpeed2Body == rhs.footSpeed2Body &&
-    lhs.footForce == rhs.footForce &&
-    lhs.footForceEst == rhs.footForceEst &&
-    lhs.tick == rhs.tick &&
-    lhs.wirelessRemote == rhs.wirelessRemote &&
-    lhs.reserve == rhs.reserve &&
-    lhs.crc == rhs.crc &&
-    lhs.eeForce == rhs.eeForce &&
-    lhs.jointP == rhs.jointP;
-}
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator!=(const ::unitree_legged_msgs::HighState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::HighState_<ContainerAllocator2> & rhs)
-{
-  return !(lhs == rhs);
-}
-
-
-} // namespace unitree_legged_msgs
-
-namespace ros
-{
-namespace message_traits
-{
-
-
-
-
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::unitree_legged_msgs::HighState_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::unitree_legged_msgs::HighState_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
-  : FalseType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::unitree_legged_msgs::HighState_<ContainerAllocator> const>
-  : FalseType
-  { };
-
-
-template<class ContainerAllocator>
-struct MD5Sum< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "a12e8b22df896c82203810ec6d521dad";
-  }
-
-  static const char* value(const ::unitree_legged_msgs::HighState_<ContainerAllocator>&) { return value(); }
-  static const uint64_t static_value1 = 0xa12e8b22df896c82ULL;
-  static const uint64_t static_value2 = 0x203810ec6d521dadULL;
-};
-
-template<class ContainerAllocator>
-struct DataType< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "unitree_legged_msgs/HighState";
-  }
-
-  static const char* value(const ::unitree_legged_msgs::HighState_<ContainerAllocator>&) { return value(); }
-};
-
-template<class ContainerAllocator>
-struct Definition< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "uint8 levelFlag\n"
-"uint16 commVersion                  # Old version Aliengo does not have\n"
-"uint16 robotID                      # Old version Aliengo does not have\n"
-"uint32 SN                           # Old version Aliengo does not have\n"
-"uint8 bandWidth                     # Old version Aliengo does not have\n"
-"uint8 mode\n"
-"IMU imu\n"
-"float32 forwardSpeed\n"
-"float32 sideSpeed\n"
-"float32 rotateSpeed\n"
-"float32 bodyHeight\n"
-"float32 updownSpeed\n"
-"float32 forwardPosition       # (will be float type next version)   # Old version Aliengo is different\n"
-"float32 sidePosition          # (will be float type next version)   # Old version Aliengo is different\n"
-"Cartesian[4] footPosition2Body\n"
-"Cartesian[4] footSpeed2Body\n"
-"int16[4] footForce                  # Old version Aliengo is different\n"
-"int16[4] footForceEst               # Old version Aliengo does not have\n"
-"uint32 tick               \n"
-"uint8[40] wirelessRemote\n"
-"uint32 reserve                      # Old version Aliengo does not have\n"
-"uint32 crc\n"
-"\n"
-"# Under are not defined in SDK yet. # Old version Aliengo does not have\n"
-"Cartesian[4] eeForce            # It's a 1-DOF force in real robot, but 3-DOF is better for visualization.\n"
-"float32[12] jointP              # for visualization\n"
-"================================================================================\n"
-"MSG: unitree_legged_msgs/IMU\n"
-"float32[4] quaternion\n"
-"float32[3] gyroscope\n"
-"float32[3] accelerometer\n"
-"int8 temperature\n"
-"================================================================================\n"
-"MSG: unitree_legged_msgs/Cartesian\n"
-"float32 x\n"
-"float32 y\n"
-"float32 z\n"
-;
-  }
-
-  static const char* value(const ::unitree_legged_msgs::HighState_<ContainerAllocator>&) { return value(); }
-};
-
-} // namespace message_traits
-} // namespace ros
-
-namespace ros
-{
-namespace serialization
-{
-
-  template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
-  {
-    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
-    {
-      stream.next(m.levelFlag);
-      stream.next(m.commVersion);
-      stream.next(m.robotID);
-      stream.next(m.SN);
-      stream.next(m.bandWidth);
-      stream.next(m.mode);
-      stream.next(m.imu);
-      stream.next(m.forwardSpeed);
-      stream.next(m.sideSpeed);
-      stream.next(m.rotateSpeed);
-      stream.next(m.bodyHeight);
-      stream.next(m.updownSpeed);
-      stream.next(m.forwardPosition);
-      stream.next(m.sidePosition);
-      stream.next(m.footPosition2Body);
-      stream.next(m.footSpeed2Body);
-      stream.next(m.footForce);
-      stream.next(m.footForceEst);
-      stream.next(m.tick);
-      stream.next(m.wirelessRemote);
-      stream.next(m.reserve);
-      stream.next(m.crc);
-      stream.next(m.eeForce);
-      stream.next(m.jointP);
-    }
-
-    ROS_DECLARE_ALLINONE_SERIALIZER
-  }; // struct HighState_
-
-} // namespace serialization
-} // namespace ros
-
-namespace ros
-{
-namespace message_operations
-{
-
-template<class ContainerAllocator>
-struct Printer< ::unitree_legged_msgs::HighState_<ContainerAllocator> >
-{
-  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::HighState_<ContainerAllocator>& v)
-  {
-    s << indent << "levelFlag: ";
-    Printer<uint8_t>::stream(s, indent + "  ", v.levelFlag);
-    s << indent << "commVersion: ";
-    Printer<uint16_t>::stream(s, indent + "  ", v.commVersion);
-    s << indent << "robotID: ";
-    Printer<uint16_t>::stream(s, indent + "  ", v.robotID);
-    s << indent << "SN: ";
-    Printer<uint32_t>::stream(s, indent + "  ", v.SN);
-    s << indent << "bandWidth: ";
-    Printer<uint8_t>::stream(s, indent + "  ", v.bandWidth);
-    s << indent << "mode: ";
-    Printer<uint8_t>::stream(s, indent + "  ", v.mode);
-    s << indent << "imu: ";
-    s << std::endl;
-    Printer< ::unitree_legged_msgs::IMU_<ContainerAllocator> >::stream(s, indent + "  ", v.imu);
-    s << indent << "forwardSpeed: ";
-    Printer<float>::stream(s, indent + "  ", v.forwardSpeed);
-    s << indent << "sideSpeed: ";
-    Printer<float>::stream(s, indent + "  ", v.sideSpeed);
-    s << indent << "rotateSpeed: ";
-    Printer<float>::stream(s, indent + "  ", v.rotateSpeed);
-    s << indent << "bodyHeight: ";
-    Printer<float>::stream(s, indent + "  ", v.bodyHeight);
-    s << indent << "updownSpeed: ";
-    Printer<float>::stream(s, indent + "  ", v.updownSpeed);
-    s << indent << "forwardPosition: ";
-    Printer<float>::stream(s, indent + "  ", v.forwardPosition);
-    s << indent << "sidePosition: ";
-    Printer<float>::stream(s, indent + "  ", v.sidePosition);
-    s << indent << "footPosition2Body[]" << std::endl;
-    for (size_t i = 0; i < v.footPosition2Body.size(); ++i)
-    {
-      s << indent << "  footPosition2Body[" << i << "]: ";
-      s << std::endl;
-      s << indent;
-      Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + "    ", v.footPosition2Body[i]);
-    }
-    s << indent << "footSpeed2Body[]" << std::endl;
-    for (size_t i = 0; i < v.footSpeed2Body.size(); ++i)
-    {
-      s << indent << "  footSpeed2Body[" << i << "]: ";
-      s << std::endl;
-      s << indent;
-      Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + "    ", v.footSpeed2Body[i]);
-    }
-    s << indent << "footForce[]" << std::endl;
-    for (size_t i = 0; i < v.footForce.size(); ++i)
-    {
-      s << indent << "  footForce[" << i << "]: ";
-      Printer<int16_t>::stream(s, indent + "  ", v.footForce[i]);
-    }
-    s << indent << "footForceEst[]" << std::endl;
-    for (size_t i = 0; i < v.footForceEst.size(); ++i)
-    {
-      s << indent << "  footForceEst[" << i << "]: ";
-      Printer<int16_t>::stream(s, indent + "  ", v.footForceEst[i]);
-    }
-    s << indent << "tick: ";
-    Printer<uint32_t>::stream(s, indent + "  ", v.tick);
-    s << indent << "wirelessRemote[]" << std::endl;
-    for (size_t i = 0; i < v.wirelessRemote.size(); ++i)
-    {
-      s << indent << "  wirelessRemote[" << i << "]: ";
-      Printer<uint8_t>::stream(s, indent + "  ", v.wirelessRemote[i]);
-    }
-    s << indent << "reserve: ";
-    Printer<uint32_t>::stream(s, indent + "  ", v.reserve);
-    s << indent << "crc: ";
-    Printer<uint32_t>::stream(s, indent + "  ", v.crc);
-    s << indent << "eeForce[]" << std::endl;
-    for (size_t i = 0; i < v.eeForce.size(); ++i)
-    {
-      s << indent << "  eeForce[" << i << "]: ";
-      s << std::endl;
-      s << indent;
-      Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + "    ", v.eeForce[i]);
-    }
-    s << indent << "jointP[]" << std::endl;
-    for (size_t i = 0; i < v.jointP.size(); ++i)
-    {
-      s << indent << "  jointP[" << i << "]: ";
-      Printer<float>::stream(s, indent + "  ", v.jointP[i]);
-    }
-  }
-};
-
-} // namespace message_operations
-} // namespace ros
-
-#endif // UNITREE_LEGGED_MSGS_MESSAGE_HIGHSTATE_H

include/message/unitree_legged_msgs/IMU.h

@@ -1,247 +0,0 @@
-// Generated by gencpp from file unitree_legged_msgs/IMU.msg
-// DO NOT EDIT!
-
-
-#ifndef UNITREE_LEGGED_MSGS_MESSAGE_IMU_H
-#define UNITREE_LEGGED_MSGS_MESSAGE_IMU_H
-
-
-#include <string>
-#include <vector>
-#include <map>
-
-#include <ros/types.h>
-#include <ros/serialization.h>
-#include <ros/builtin_message_traits.h>
-#include <ros/message_operations.h>
-
-
-namespace unitree_legged_msgs
-{
-template <class ContainerAllocator>
-struct IMU_
-{
-  typedef IMU_<ContainerAllocator> Type;
-
-  IMU_()
-    : quaternion()
-    , gyroscope()
-    , accelerometer()
-    , temperature(0)  {
-      quaternion.assign(0.0);
-
-      gyroscope.assign(0.0);
-
-      accelerometer.assign(0.0);
-  }
-  IMU_(const ContainerAllocator& _alloc)
-    : quaternion()
-    , gyroscope()
-    , accelerometer()
-    , temperature(0)  {
-  (void)_alloc;
-      quaternion.assign(0.0);
-
-      gyroscope.assign(0.0);
-
-      accelerometer.assign(0.0);
-  }
-
-
-
-   typedef boost::array<float, 4>  _quaternion_type;
-  _quaternion_type quaternion;
-
-   typedef boost::array<float, 3>  _gyroscope_type;
-  _gyroscope_type gyroscope;
-
-   typedef boost::array<float, 3>  _accelerometer_type;
-  _accelerometer_type accelerometer;
-
-   typedef int8_t _temperature_type;
-  _temperature_type temperature;
-
-
-
-
-
-  typedef boost::shared_ptr< ::unitree_legged_msgs::IMU_<ContainerAllocator> > Ptr;
-  typedef boost::shared_ptr< ::unitree_legged_msgs::IMU_<ContainerAllocator> const> ConstPtr;
-
-}; // struct IMU_
-
-typedef ::unitree_legged_msgs::IMU_<std::allocator<void> > IMU;
-
-typedef boost::shared_ptr< ::unitree_legged_msgs::IMU > IMUPtr;
-typedef boost::shared_ptr< ::unitree_legged_msgs::IMU const> IMUConstPtr;
-
-// constants requiring out of line definition
-
-
-
-template<typename ContainerAllocator>
-std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::IMU_<ContainerAllocator> & v)
-{
-ros::message_operations::Printer< ::unitree_legged_msgs::IMU_<ContainerAllocator> >::stream(s, "", v);
-return s;
-}
-
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator==(const ::unitree_legged_msgs::IMU_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::IMU_<ContainerAllocator2> & rhs)
-{
-  return lhs.quaternion == rhs.quaternion &&
-    lhs.gyroscope == rhs.gyroscope &&
-    lhs.accelerometer == rhs.accelerometer &&
-    lhs.temperature == rhs.temperature;
-}
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator!=(const ::unitree_legged_msgs::IMU_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::IMU_<ContainerAllocator2> & rhs)
-{
-  return !(lhs == rhs);
-}
-
-
-} // namespace unitree_legged_msgs
-
-namespace ros
-{
-namespace message_traits
-{
-
-
-
-
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::unitree_legged_msgs::IMU_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::unitree_legged_msgs::IMU_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
-  : FalseType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::unitree_legged_msgs::IMU_<ContainerAllocator> const>
-  : FalseType
-  { };
-
-
-template<class ContainerAllocator>
-struct MD5Sum< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "dd4bb4e42aa2f15aa1fb1b6a3c3752cb";
-  }
-
-  static const char* value(const ::unitree_legged_msgs::IMU_<ContainerAllocator>&) { return value(); }
-  static const uint64_t static_value1 = 0xdd4bb4e42aa2f15aULL;
-  static const uint64_t static_value2 = 0xa1fb1b6a3c3752cbULL;
-};
-
-template<class ContainerAllocator>
-struct DataType< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "unitree_legged_msgs/IMU";
-  }
-
-  static const char* value(const ::unitree_legged_msgs::IMU_<ContainerAllocator>&) { return value(); }
-};
-
-template<class ContainerAllocator>
-struct Definition< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "float32[4] quaternion\n"
-"float32[3] gyroscope\n"
-"float32[3] accelerometer\n"
-"int8 temperature\n"
-;
-  }
-
-  static const char* value(const ::unitree_legged_msgs::IMU_<ContainerAllocator>&) { return value(); }
-};
-
-} // namespace message_traits
-} // namespace ros
-
-namespace ros
-{
-namespace serialization
-{
-
-  template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
-  {
-    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
-    {
-      stream.next(m.quaternion);
-      stream.next(m.gyroscope);
-      stream.next(m.accelerometer);
-      stream.next(m.temperature);
-    }
-
-    ROS_DECLARE_ALLINONE_SERIALIZER
-  }; // struct IMU_
-
-} // namespace serialization
-} // namespace ros
-
-namespace ros
-{
-namespace message_operations
-{
-
-template<class ContainerAllocator>
-struct Printer< ::unitree_legged_msgs::IMU_<ContainerAllocator> >
-{
-  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::IMU_<ContainerAllocator>& v)
-  {
-    s << indent << "quaternion[]" << std::endl;
-    for (size_t i = 0; i < v.quaternion.size(); ++i)
-    {
-      s << indent << "  quaternion[" << i << "]: ";
-      Printer<float>::stream(s, indent + "  ", v.quaternion[i]);
-    }
-    s << indent << "gyroscope[]" << std::endl;
-    for (size_t i = 0; i < v.gyroscope.size(); ++i)
-    {
-      s << indent << "  gyroscope[" << i << "]: ";
-      Printer<float>::stream(s, indent + "  ", v.gyroscope[i]);
-    }
-    s << indent << "accelerometer[]" << std::endl;
-    for (size_t i = 0; i < v.accelerometer.size(); ++i)
-    {
-      s << indent << "  accelerometer[" << i << "]: ";
-      Printer<float>::stream(s, indent + "  ", v.accelerometer[i]);
-    }
-    s << indent << "temperature: ";
-    Printer<int8_t>::stream(s, indent + "  ", v.temperature);
-  }
-};
-
-} // namespace message_operations
-} // namespace ros
-
-#endif // UNITREE_LEGGED_MSGS_MESSAGE_IMU_H

include/message/unitree_legged_msgs/LED.h

@@ -1,215 +0,0 @@
-// Generated by gencpp from file unitree_legged_msgs/LED.msg
-// DO NOT EDIT!
-
-
-#ifndef UNITREE_LEGGED_MSGS_MESSAGE_LED_H
-#define UNITREE_LEGGED_MSGS_MESSAGE_LED_H
-
-
-#include <string>
-#include <vector>
-#include <map>
-
-#include <ros/types.h>
-#include <ros/serialization.h>
-#include <ros/builtin_message_traits.h>
-#include <ros/message_operations.h>
-
-
-namespace unitree_legged_msgs
-{
-template <class ContainerAllocator>
-struct LED_
-{
-  typedef LED_<ContainerAllocator> Type;
-
-  LED_()
-    : r(0)
-    , g(0)
-    , b(0)  {
-    }
-  LED_(const ContainerAllocator& _alloc)
-    : r(0)
-    , g(0)
-    , b(0)  {
-  (void)_alloc;
-    }
-
-
-
-   typedef uint8_t _r_type;
-  _r_type r;
-
-   typedef uint8_t _g_type;
-  _g_type g;
-
-   typedef uint8_t _b_type;
-  _b_type b;
-
-
-
-
-
-  typedef boost::shared_ptr< ::unitree_legged_msgs::LED_<ContainerAllocator> > Ptr;
-  typedef boost::shared_ptr< ::unitree_legged_msgs::LED_<ContainerAllocator> const> ConstPtr;
-
-}; // struct LED_
-
-typedef ::unitree_legged_msgs::LED_<std::allocator<void> > LED;
-
-typedef boost::shared_ptr< ::unitree_legged_msgs::LED > LEDPtr;
-typedef boost::shared_ptr< ::unitree_legged_msgs::LED const> LEDConstPtr;
-
-// constants requiring out of line definition
-
-
-
-template<typename ContainerAllocator>
-std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LED_<ContainerAllocator> & v)
-{
-ros::message_operations::Printer< ::unitree_legged_msgs::LED_<ContainerAllocator> >::stream(s, "", v);
-return s;
-}
-
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator==(const ::unitree_legged_msgs::LED_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LED_<ContainerAllocator2> & rhs)
-{
-  return lhs.r == rhs.r &&
-    lhs.g == rhs.g &&
-    lhs.b == rhs.b;
-}
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator!=(const ::unitree_legged_msgs::LED_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LED_<ContainerAllocator2> & rhs)
-{
-  return !(lhs == rhs);
-}
-
-
-} // namespace unitree_legged_msgs
-
-namespace ros
-{
-namespace message_traits
-{
-
-
-
-
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::unitree_legged_msgs::LED_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::unitree_legged_msgs::LED_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::unitree_legged_msgs::LED_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::unitree_legged_msgs::LED_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::unitree_legged_msgs::LED_<ContainerAllocator> >
-  : FalseType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::unitree_legged_msgs::LED_<ContainerAllocator> const>
-  : FalseType
-  { };
-
-
-template<class ContainerAllocator>
-struct MD5Sum< ::unitree_legged_msgs::LED_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "353891e354491c51aabe32df673fb446";
-  }
-
-  static const char* value(const ::unitree_legged_msgs::LED_<ContainerAllocator>&) { return value(); }
-  static const uint64_t static_value1 = 0x353891e354491c51ULL;
-  static const uint64_t static_value2 = 0xaabe32df673fb446ULL;
-};
-
-template<class ContainerAllocator>
-struct DataType< ::unitree_legged_msgs::LED_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "unitree_legged_msgs/LED";
-  }
-
-  static const char* value(const ::unitree_legged_msgs::LED_<ContainerAllocator>&) { return value(); }
-};
-
-template<class ContainerAllocator>
-struct Definition< ::unitree_legged_msgs::LED_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "uint8 r\n"
-"uint8 g\n"
-"uint8 b\n"
-;
-  }
-
-  static const char* value(const ::unitree_legged_msgs::LED_<ContainerAllocator>&) { return value(); }
-};
-
-} // namespace message_traits
-} // namespace ros
-
-namespace ros
-{
-namespace serialization
-{
-
-  template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::LED_<ContainerAllocator> >
-  {
-    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
-    {
-      stream.next(m.r);
-      stream.next(m.g);
-      stream.next(m.b);
-    }
-
-    ROS_DECLARE_ALLINONE_SERIALIZER
-  }; // struct LED_
-
-} // namespace serialization
-} // namespace ros
-
-namespace ros
-{
-namespace message_operations
-{
-
-template<class ContainerAllocator>
-struct Printer< ::unitree_legged_msgs::LED_<ContainerAllocator> >
-{
-  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LED_<ContainerAllocator>& v)
-  {
-    s << indent << "r: ";
-    Printer<uint8_t>::stream(s, indent + "  ", v.r);
-    s << indent << "g: ";
-    Printer<uint8_t>::stream(s, indent + "  ", v.g);
-    s << indent << "b: ";
-    Printer<uint8_t>::stream(s, indent + "  ", v.b);
-  }
-};
-
-} // namespace message_operations
-} // namespace ros
-
-#endif // UNITREE_LEGGED_MSGS_MESSAGE_LED_H

include/message/unitree_legged_msgs/LowCmd.h

@@ -1,348 +0,0 @@
-// Generated by gencpp from file unitree_legged_msgs/LowCmd.msg
-// DO NOT EDIT!
-
-
-#ifndef UNITREE_LEGGED_MSGS_MESSAGE_LOWCMD_H
-#define UNITREE_LEGGED_MSGS_MESSAGE_LOWCMD_H
-
-
-#include <string>
-#include <vector>
-#include <map>
-
-#include <ros/types.h>
-#include <ros/serialization.h>
-#include <ros/builtin_message_traits.h>
-#include <ros/message_operations.h>
-
-#include <unitree_legged_msgs/MotorCmd.h>
-#include <unitree_legged_msgs/LED.h>
-#include <unitree_legged_msgs/Cartesian.h>
-
-namespace unitree_legged_msgs
-{
-template <class ContainerAllocator>
-struct LowCmd_
-{
-  typedef LowCmd_<ContainerAllocator> Type;
-
-  LowCmd_()
-    : levelFlag(0)
-    , commVersion(0)
-    , robotID(0)
-    , SN(0)
-    , bandWidth(0)
-    , motorCmd()
-    , led()
-    , wirelessRemote()
-    , reserve(0)
-    , crc(0)
-    , ff()  {
-      wirelessRemote.assign(0);
-  }
-  LowCmd_(const ContainerAllocator& _alloc)
-    : levelFlag(0)
-    , commVersion(0)
-    , robotID(0)
-    , SN(0)
-    , bandWidth(0)
-    , motorCmd()
-    , led()
-    , wirelessRemote()
-    , reserve(0)
-    , crc(0)
-    , ff()  {
-  (void)_alloc;
-      motorCmd.assign( ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> (_alloc));
-
-      led.assign( ::unitree_legged_msgs::LED_<ContainerAllocator> (_alloc));
-
-      wirelessRemote.assign(0);
-
-      ff.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
-  }
-
-
-
-   typedef uint8_t _levelFlag_type;
-  _levelFlag_type levelFlag;
-
-   typedef uint16_t _commVersion_type;
-  _commVersion_type commVersion;
-
-   typedef uint16_t _robotID_type;
-  _robotID_type robotID;
-
-   typedef uint32_t _SN_type;
-  _SN_type SN;
-
-   typedef uint8_t _bandWidth_type;
-  _bandWidth_type bandWidth;
-
-   typedef boost::array< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> , 20>  _motorCmd_type;
-  _motorCmd_type motorCmd;
-
-   typedef boost::array< ::unitree_legged_msgs::LED_<ContainerAllocator> , 4>  _led_type;
-  _led_type led;
-
-   typedef boost::array<uint8_t, 40>  _wirelessRemote_type;
-  _wirelessRemote_type wirelessRemote;
-
-   typedef uint32_t _reserve_type;
-  _reserve_type reserve;
-
-   typedef uint32_t _crc_type;
-  _crc_type crc;
-
-   typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4>  _ff_type;
-  _ff_type ff;
-
-
-
-
-
-  typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> > Ptr;
-  typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> const> ConstPtr;
-
-}; // struct LowCmd_
-
-typedef ::unitree_legged_msgs::LowCmd_<std::allocator<void> > LowCmd;
-
-typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd > LowCmdPtr;
-typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd const> LowCmdConstPtr;
-
-// constants requiring out of line definition
-
-
-
-template<typename ContainerAllocator>
-std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LowCmd_<ContainerAllocator> & v)
-{
-ros::message_operations::Printer< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >::stream(s, "", v);
-return s;
-}
-
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator==(const ::unitree_legged_msgs::LowCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LowCmd_<ContainerAllocator2> & rhs)
-{
-  return lhs.levelFlag == rhs.levelFlag &&
-    lhs.commVersion == rhs.commVersion &&
-    lhs.robotID == rhs.robotID &&
-    lhs.SN == rhs.SN &&
-    lhs.bandWidth == rhs.bandWidth &&
-    lhs.motorCmd == rhs.motorCmd &&
-    lhs.led == rhs.led &&
-    lhs.wirelessRemote == rhs.wirelessRemote &&
-    lhs.reserve == rhs.reserve &&
-    lhs.crc == rhs.crc &&
-    lhs.ff == rhs.ff;
-}
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator!=(const ::unitree_legged_msgs::LowCmd_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LowCmd_<ContainerAllocator2> & rhs)
-{
-  return !(lhs == rhs);
-}
-
-
-} // namespace unitree_legged_msgs
-
-namespace ros
-{
-namespace message_traits
-{
-
-
-
-
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
-  : FalseType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> const>
-  : FalseType
-  { };
-
-
-template<class ContainerAllocator>
-struct MD5Sum< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "357432b2562edd0a8e89b9c9f5fc4821";
-  }
-
-  static const char* value(const ::unitree_legged_msgs::LowCmd_<ContainerAllocator>&) { return value(); }
-  static const uint64_t static_value1 = 0x357432b2562edd0aULL;
-  static const uint64_t static_value2 = 0x8e89b9c9f5fc4821ULL;
-};
-
-template<class ContainerAllocator>
-struct DataType< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "unitree_legged_msgs/LowCmd";
-  }
-
-  static const char* value(const ::unitree_legged_msgs::LowCmd_<ContainerAllocator>&) { return value(); }
-};
-
-template<class ContainerAllocator>
-struct Definition< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "uint8 levelFlag                 \n"
-"uint16 commVersion              # Old version Aliengo does not have\n"
-"uint16 robotID                  # Old version Aliengo does not have\n"
-"uint32 SN                       # Old version Aliengo does not have\n"
-"uint8 bandWidth                 # Old version Aliengo does not have\n"
-"MotorCmd[20] motorCmd\n"
-"LED[4] led\n"
-"uint8[40] wirelessRemote\n"
-"uint32 reserve                  # Old version Aliengo does not have\n"
-"uint32 crc\n"
-"\n"
-"Cartesian[4] ff               # will delete # Old version Aliengo does not have\n"
-"================================================================================\n"
-"MSG: unitree_legged_msgs/MotorCmd\n"
-"uint8 mode           # motor target mode\n"
-"float32 q            # motor target position\n"
-"float32 dq           # motor target velocity\n"
-"float32 tau          # motor target torque\n"
-"float32 Kp           # motor spring stiffness coefficient\n"
-"float32 Kd           # motor damper coefficient\n"
-"uint32[3] reserve    # motor target torque\n"
-"================================================================================\n"
-"MSG: unitree_legged_msgs/LED\n"
-"uint8 r\n"
-"uint8 g\n"
-"uint8 b\n"
-"================================================================================\n"
-"MSG: unitree_legged_msgs/Cartesian\n"
-"float32 x\n"
-"float32 y\n"
-"float32 z\n"
-;
-  }
-
-  static const char* value(const ::unitree_legged_msgs::LowCmd_<ContainerAllocator>&) { return value(); }
-};
-
-} // namespace message_traits
-} // namespace ros
-
-namespace ros
-{
-namespace serialization
-{
-
-  template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
-  {
-    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
-    {
-      stream.next(m.levelFlag);
-      stream.next(m.commVersion);
-      stream.next(m.robotID);
-      stream.next(m.SN);
-      stream.next(m.bandWidth);
-      stream.next(m.motorCmd);
-      stream.next(m.led);
-      stream.next(m.wirelessRemote);
-      stream.next(m.reserve);
-      stream.next(m.crc);
-      stream.next(m.ff);
-    }
-
-    ROS_DECLARE_ALLINONE_SERIALIZER
-  }; // struct LowCmd_
-
-} // namespace serialization
-} // namespace ros
-
-namespace ros
-{
-namespace message_operations
-{
-
-template<class ContainerAllocator>
-struct Printer< ::unitree_legged_msgs::LowCmd_<ContainerAllocator> >
-{
-  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LowCmd_<ContainerAllocator>& v)
-  {
-    s << indent << "levelFlag: ";
-    Printer<uint8_t>::stream(s, indent + "  ", v.levelFlag);
-    s << indent << "commVersion: ";
-    Printer<uint16_t>::stream(s, indent + "  ", v.commVersion);
-    s << indent << "robotID: ";
-    Printer<uint16_t>::stream(s, indent + "  ", v.robotID);
-    s << indent << "SN: ";
-    Printer<uint32_t>::stream(s, indent + "  ", v.SN);
-    s << indent << "bandWidth: ";
-    Printer<uint8_t>::stream(s, indent + "  ", v.bandWidth);
-    s << indent << "motorCmd[]" << std::endl;
-    for (size_t i = 0; i < v.motorCmd.size(); ++i)
-    {
-      s << indent << "  motorCmd[" << i << "]: ";
-      s << std::endl;
-      s << indent;
-      Printer< ::unitree_legged_msgs::MotorCmd_<ContainerAllocator> >::stream(s, indent + "    ", v.motorCmd[i]);
-    }
-    s << indent << "led[]" << std::endl;
-    for (size_t i = 0; i < v.led.size(); ++i)
-    {
-      s << indent << "  led[" << i << "]: ";
-      s << std::endl;
-      s << indent;
-      Printer< ::unitree_legged_msgs::LED_<ContainerAllocator> >::stream(s, indent + "    ", v.led[i]);
-    }
-    s << indent << "wirelessRemote[]" << std::endl;
-    for (size_t i = 0; i < v.wirelessRemote.size(); ++i)
-    {
-      s << indent << "  wirelessRemote[" << i << "]: ";
-      Printer<uint8_t>::stream(s, indent + "  ", v.wirelessRemote[i]);
-    }
-    s << indent << "reserve: ";
-    Printer<uint32_t>::stream(s, indent + "  ", v.reserve);
-    s << indent << "crc: ";
-    Printer<uint32_t>::stream(s, indent + "  ", v.crc);
-    s << indent << "ff[]" << std::endl;
-    for (size_t i = 0; i < v.ff.size(); ++i)
-    {
-      s << indent << "  ff[" << i << "]: ";
-      s << std::endl;
-      s << indent;
-      Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + "    ", v.ff[i]);
-    }
-  }
-};
-
-} // namespace message_operations
-} // namespace ros
-
-#endif // UNITREE_LEGGED_MSGS_MESSAGE_LOWCMD_H

include/message/unitree_legged_msgs/LowState.h

@@ -1,448 +0,0 @@
-// Generated by gencpp from file unitree_legged_msgs/LowState.msg
-// DO NOT EDIT!
-
-
-#ifndef UNITREE_LEGGED_MSGS_MESSAGE_LOWSTATE_H
-#define UNITREE_LEGGED_MSGS_MESSAGE_LOWSTATE_H
-
-
-#include <string>
-#include <vector>
-#include <map>
-
-#include <ros/types.h>
-#include <ros/serialization.h>
-#include <ros/builtin_message_traits.h>
-#include <ros/message_operations.h>
-
-#include <unitree_legged_msgs/IMU.h>
-#include <unitree_legged_msgs/MotorState.h>
-#include <unitree_legged_msgs/Cartesian.h>
-#include <unitree_legged_msgs/Cartesian.h>
-#include <unitree_legged_msgs/Cartesian.h>
-#include <unitree_legged_msgs/Cartesian.h>
-#include <unitree_legged_msgs/Cartesian.h>
-
-namespace unitree_legged_msgs
-{
-template <class ContainerAllocator>
-struct LowState_
-{
-  typedef LowState_<ContainerAllocator> Type;
-
-  LowState_()
-    : levelFlag(0)
-    , commVersion(0)
-    , robotID(0)
-    , SN(0)
-    , bandWidth(0)
-    , imu()
-    , motorState()
-    , footForce()
-    , footForceEst()
-    , tick(0)
-    , wirelessRemote()
-    , reserve(0)
-    , crc(0)
-    , eeForceRaw()
-    , eeForce()
-    , position()
-    , velocity()
-    , velocity_w()  {
-      footForce.assign(0);
-
-      footForceEst.assign(0);
-
-      wirelessRemote.assign(0);
-  }
-  LowState_(const ContainerAllocator& _alloc)
-    : levelFlag(0)
-    , commVersion(0)
-    , robotID(0)
-    , SN(0)
-    , bandWidth(0)
-    , imu(_alloc)
-    , motorState()
-    , footForce()
-    , footForceEst()
-    , tick(0)
-    , wirelessRemote()
-    , reserve(0)
-    , crc(0)
-    , eeForceRaw()
-    , eeForce()
-    , position(_alloc)
-    , velocity(_alloc)
-    , velocity_w(_alloc)  {
-  (void)_alloc;
-      motorState.assign( ::unitree_legged_msgs::MotorState_<ContainerAllocator> (_alloc));
-
-      footForce.assign(0);
-
-      footForceEst.assign(0);
-
-      wirelessRemote.assign(0);
-
-      eeForceRaw.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
-
-      eeForce.assign( ::unitree_legged_msgs::Cartesian_<ContainerAllocator> (_alloc));
-  }
-
-
-
-   typedef uint8_t _levelFlag_type;
-  _levelFlag_type levelFlag;
-
-   typedef uint16_t _commVersion_type;
-  _commVersion_type commVersion;
-
-   typedef uint16_t _robotID_type;
-  _robotID_type robotID;
-
-   typedef uint32_t _SN_type;
-  _SN_type SN;
-
-   typedef uint8_t _bandWidth_type;
-  _bandWidth_type bandWidth;
-
-   typedef  ::unitree_legged_msgs::IMU_<ContainerAllocator>  _imu_type;
-  _imu_type imu;
-
-   typedef boost::array< ::unitree_legged_msgs::MotorState_<ContainerAllocator> , 20>  _motorState_type;
-  _motorState_type motorState;
-
-   typedef boost::array<int16_t, 4>  _footForce_type;
-  _footForce_type footForce;
-
-   typedef boost::array<int16_t, 4>  _footForceEst_type;
-  _footForceEst_type footForceEst;
-
-   typedef uint32_t _tick_type;
-  _tick_type tick;
-
-   typedef boost::array<uint8_t, 40>  _wirelessRemote_type;
-  _wirelessRemote_type wirelessRemote;
-
-   typedef uint32_t _reserve_type;
-  _reserve_type reserve;
-
-   typedef uint32_t _crc_type;
-  _crc_type crc;
-
-   typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4>  _eeForceRaw_type;
-  _eeForceRaw_type eeForceRaw;
-
-   typedef boost::array< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> , 4>  _eeForce_type;
-  _eeForce_type eeForce;
-
-   typedef  ::unitree_legged_msgs::Cartesian_<ContainerAllocator>  _position_type;
-  _position_type position;
-
-   typedef  ::unitree_legged_msgs::Cartesian_<ContainerAllocator>  _velocity_type;
-  _velocity_type velocity;
-
-   typedef  ::unitree_legged_msgs::Cartesian_<ContainerAllocator>  _velocity_w_type;
-  _velocity_w_type velocity_w;
-
-
-
-
-
-  typedef boost::shared_ptr< ::unitree_legged_msgs::LowState_<ContainerAllocator> > Ptr;
-  typedef boost::shared_ptr< ::unitree_legged_msgs::LowState_<ContainerAllocator> const> ConstPtr;
-
-}; // struct LowState_
-
-typedef ::unitree_legged_msgs::LowState_<std::allocator<void> > LowState;
-
-typedef boost::shared_ptr< ::unitree_legged_msgs::LowState > LowStatePtr;
-typedef boost::shared_ptr< ::unitree_legged_msgs::LowState const> LowStateConstPtr;
-
-// constants requiring out of line definition
-
-
-
-template<typename ContainerAllocator>
-std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LowState_<ContainerAllocator> & v)
-{
-ros::message_operations::Printer< ::unitree_legged_msgs::LowState_<ContainerAllocator> >::stream(s, "", v);
-return s;
-}
-
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator==(const ::unitree_legged_msgs::LowState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LowState_<ContainerAllocator2> & rhs)
-{
-  return lhs.levelFlag == rhs.levelFlag &&
-    lhs.commVersion == rhs.commVersion &&
-    lhs.robotID == rhs.robotID &&
-    lhs.SN == rhs.SN &&
-    lhs.bandWidth == rhs.bandWidth &&
-    lhs.imu == rhs.imu &&
-    lhs.motorState == rhs.motorState &&
-    lhs.footForce == rhs.footForce &&
-    lhs.footForceEst == rhs.footForceEst &&
-    lhs.tick == rhs.tick &&
-    lhs.wirelessRemote == rhs.wirelessRemote &&
-    lhs.reserve == rhs.reserve &&
-    lhs.crc == rhs.crc &&
-    lhs.eeForceRaw == rhs.eeForceRaw &&
-    lhs.eeForce == rhs.eeForce &&
-    lhs.position == rhs.position &&
-    lhs.velocity == rhs.velocity &&
-    lhs.velocity_w == rhs.velocity_w;
-}
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator!=(const ::unitree_legged_msgs::LowState_<ContainerAllocator1> & lhs, const ::unitree_legged_msgs::LowState_<ContainerAllocator2> & rhs)
-{
-  return !(lhs == rhs);
-}
-
-
-} // namespace unitree_legged_msgs
-
-namespace ros
-{
-namespace message_traits
-{
-
-
-
-
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::unitree_legged_msgs::LowState_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::unitree_legged_msgs::LowState_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
-  : FalseType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::unitree_legged_msgs::LowState_<ContainerAllocator> const>
-  : FalseType
-  { };
-
-
-template<class ContainerAllocator>
-struct MD5Sum< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "cef9d4f6b5a89bd6330896affb1bca88";
-  }
-
-  static const char* value(const ::unitree_legged_msgs::LowState_<ContainerAllocator>&) { return value(); }
-  static const uint64_t static_value1 = 0xcef9d4f6b5a89bd6ULL;
-  static const uint64_t static_value2 = 0x330896affb1bca88ULL;
-};
-
-template<class ContainerAllocator>
-struct DataType< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "unitree_legged_msgs/LowState";
-  }
-
-  static const char* value(const ::unitree_legged_msgs::LowState_<ContainerAllocator>&) { return value(); }
-};
-
-template<class ContainerAllocator>
-struct Definition< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "uint8 levelFlag\n"
-"uint16 commVersion              # Old version Aliengo does not have\n"
-"uint16 robotID                  # Old version Aliengo does not have\n"
-"uint32 SN                       # Old version Aliengo does not have\n"
-"uint8 bandWidth                 # Old version Aliengo does not have\n"
-"IMU imu\n"
-"MotorState[20] motorState\n"
-"int16[4] footForce              # force sensors     # Old version Aliengo is different\n"
-"int16[4] footForceEst           # force sensors     # Old version Aliengo does not have\n"
-"uint32 tick                     # reference real-time from motion controller (unit: us)\n"
-"uint8[40] wirelessRemote        # wireless commands\n"
-"uint32 reserve                  # Old version Aliengo does not have\n"
-"uint32 crc\n"
-"\n"
-"# Old version Aliengo does not have:\n"
-"Cartesian[4] eeForceRaw\n"
-"Cartesian[4] eeForce          #it's a 1-DOF force infact, but we use 3-DOF here just for visualization \n"
-"Cartesian position            # will delete\n"
-"Cartesian velocity            # will delete\n"
-"Cartesian velocity_w            # will delete\n"
-"\n"
-"================================================================================\n"
-"MSG: unitree_legged_msgs/IMU\n"
-"float32[4] quaternion\n"
-"float32[3] gyroscope\n"
-"float32[3] accelerometer\n"
-"int8 temperature\n"
-"================================================================================\n"
-"MSG: unitree_legged_msgs/MotorState\n"
-"uint8 mode           # motor current mode \n"
-"float32 q            # motor current position（rad）\n"
-"float32 dq           # motor current speed（rad/s）\n"
-"float32 ddq          # motor current speed（rad/s）\n"
-"float32 tauEst       # current estimated output torque（N*m）\n"
-"float32 q_raw        # motor current position（rad）\n"
-"float32 dq_raw       # motor current speed（rad/s）\n"
-"float32 ddq_raw      # motor current speed（rad/s）\n"
-"int8 temperature     # motor temperature（slow conduction of temperature leads to lag）\n"
-"uint32[2] reserve\n"
-"================================================================================\n"
-"MSG: unitree_legged_msgs/Cartesian\n"
-"float32 x\n"
-"float32 y\n"
-"float32 z\n"
-;
-  }
-
-  static const char* value(const ::unitree_legged_msgs::LowState_<ContainerAllocator>&) { return value(); }
-};
-
-} // namespace message_traits
-} // namespace ros
-
-namespace ros
-{
-namespace serialization
-{
-
-  template<class ContainerAllocator> struct Serializer< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
-  {
-    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
-    {
-      stream.next(m.levelFlag);
-      stream.next(m.commVersion);
-      stream.next(m.robotID);
-      stream.next(m.SN);
-      stream.next(m.bandWidth);
-      stream.next(m.imu);
-      stream.next(m.motorState);
-      stream.next(m.footForce);
-      stream.next(m.footForceEst);
-      stream.next(m.tick);
-      stream.next(m.wirelessRemote);
-      stream.next(m.reserve);
-      stream.next(m.crc);
-      stream.next(m.eeForceRaw);
-      stream.next(m.eeForce);
-      stream.next(m.position);
-      stream.next(m.velocity);
-      stream.next(m.velocity_w);
-    }
-
-    ROS_DECLARE_ALLINONE_SERIALIZER
-  }; // struct LowState_
-
-} // namespace serialization
-} // namespace ros
-
-namespace ros
-{
-namespace message_operations
-{
-
-template<class ContainerAllocator>
-struct Printer< ::unitree_legged_msgs::LowState_<ContainerAllocator> >
-{
-  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LowState_<ContainerAllocator>& v)
-  {
-    s << indent << "levelFlag: ";
-    Printer<uint8_t>::stream(s, indent + "  ", v.levelFlag);
-    s << indent << "commVersion: ";
-    Printer<uint16_t>::stream(s, indent + "  ", v.commVersion);
-    s << indent << "robotID: ";
-    Printer<uint16_t>::stream(s, indent + "  ", v.robotID);
-    s << indent << "SN: ";
-    Printer<uint32_t>::stream(s, indent + "  ", v.SN);
-    s << indent << "bandWidth: ";
-    Printer<uint8_t>::stream(s, indent + "  ", v.bandWidth);
-    s << indent << "imu: ";
-    s << std::endl;
-    Printer< ::unitree_legged_msgs::IMU_<ContainerAllocator> >::stream(s, indent + "  ", v.imu);
-    s << indent << "motorState[]" << std::endl;
-    for (size_t i = 0; i < v.motorState.size(); ++i)
-    {
-      s << indent << "  motorState[" << i << "]: ";
-      s << std::endl;
-      s << indent;
-      Printer< ::unitree_legged_msgs::MotorState_<ContainerAllocator> >::stream(s, indent + "    ", v.motorState[i]);
-    }
-    s << indent << "footForce[]" << std::endl;
-    for (size_t i = 0; i < v.footForce.size(); ++i)
-    {
-      s << indent << "  footForce[" << i << "]: ";
-      Printer<int16_t>::stream(s, indent + "  ", v.footForce[i]);
-    }
-    s << indent << "footForceEst[]" << std::endl;
-    for (size_t i = 0; i < v.footForceEst.size(); ++i)
-    {
-      s << indent << "  footForceEst[" << i << "]: ";
-      Printer<int16_t>::stream(s, indent + "  ", v.footForceEst[i]);
-    }
-    s << indent << "tick: ";
-    Printer<uint32_t>::stream(s, indent + "  ", v.tick);
-    s << indent << "wirelessRemote[]" << std::endl;
-    for (size_t i = 0; i < v.wirelessRemote.size(); ++i)
-    {
-      s << indent << "  wirelessRemote[" << i << "]: ";
-      Printer<uint8_t>::stream(s, indent + "  ", v.wirelessRemote[i]);
-    }
-    s << indent << "reserve: ";
-    Printer<uint32_t>::stream(s, indent + "  ", v.reserve);
-    s << indent << "crc: ";
-    Printer<uint32_t>::stream(s, indent + "  ", v.crc);
-    s << indent << "eeForceRaw[]" << std::endl;
-    for (size_t i = 0; i < v.eeForceRaw.size(); ++i)
-    {
-      s << indent << "  eeForceRaw[" << i << "]: ";
-      s << std::endl;
-      s << indent;
-      Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + "    ", v.eeForceRaw[i]);
-    }
-    s << indent << "eeForce[]" << std::endl;
-    for (size_t i = 0; i < v.eeForce.size(); ++i)
-    {
-      s << indent << "  eeForce[" << i << "]: ";
-      s << std::endl;
-      s << indent;
-      Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + "    ", v.eeForce[i]);
-    }
-    s << indent << "position: ";
-    s << std::endl;
-    Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + "  ", v.position);
-    s << indent << "velocity: ";
-    s << std::endl;
-    Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + "  ", v.velocity);
-    s << indent << "velocity_w: ";
-    s << std::endl;
-    Printer< ::unitree_legged_msgs::Cartesian_<ContainerAllocator> >::stream(s, indent + "  ", v.velocity_w);
-  }
-};
-
-} // namespace message_operations
-} // namespace ros
-
-#endif // UNITREE_LEGGED_MSGS_MESSAGE_LOWSTATE_H

include/model/ArmDynKineModel.h

@@ -1,124 +0,0 @@
-
-#ifndef ARMDYNCKINEMODEL_H
-#define ARMDYNCKINEMODEL_H
-
-#include <rbdl/rbdl.h>
-#include <vector>
-#include "common/math/mathTypes.h"
-#include "quadProgpp/include/QuadProg++.hh"
-
-/* Kinematics only suitable for 6 DOF arm */
-class ArmDynKineModel{
-public:
-    ArmDynKineModel(int dof, Vec3 endPosLocal,
-        double endEffectorMass,
-        Vec3 endEffectorCom, Mat3 endEffectorInertia);
-    virtual ~ArmDynKineModel();
-
-    Vec6 forDerivativeKinematice(Vec6 q, Vec6 qd);
-    Vec6 invDerivativeKinematice(Vec6 q, Vec6 twistSpatial);
-    Vec6 invDynamics(Vec6 q, Vec6 qd, Vec6 qdd, Vec6 endForce);
-
-    int getDOF(){return _dof;}
-    std::vector<double> getJointQMax(){return _jointQMax;}
-    std::vector<double> getJointQMin(){return _jointQMin;}
-    std::vector<double> getJointSpeedMax(){return _jointSpeedMax;}
-
-    void solveQP(Vec6 twist, Vec6 pastAngle, Vec6 &result, double dt, bool setJ0QdZero, bool setQdZero);
-    bool invKinematics(HomoMat gDes, Vec6 pastAngle, Vec6 &result, bool checkInWorkSpace = false);
-    HomoMat forwardKinematics(Vec6 q, int index=6, bool isExp = false);   // index from 0 to 5, used for fourth version
-    bool _useIKSolver = false;
-
-protected:
-    void _dofCheck();
-    void _buildModel();
-
-    int _dof;
-    RigidBodyDynamics::Model *_model;
-    std::vector<unsigned int> _linkID;
-
-    std::vector<RigidBodyDynamics::Body> _body;
-    std::vector<double> _linkMass;
-    std::vector<Vec3> _linkCom;
-    std::vector<RigidBodyDynamics::Math::Matrix3d> _linkInertia;
-    double _endEffectorMass;
-    Vec3 _endEffectorCom;
-    Mat3 _endEffectorInertia;
-
-    std::vector<RigidBodyDynamics::Joint> _joint;
-    std::vector<Vec3> _jointAxis;
-    std::vector<Vec3> _jointPos;
-    std::vector<double> _jointQMax;
-    std::vector<double> _jointQMin;
-    std::vector<double> _jointSpeedMax;
-    Vec3 _endPosGlobal, _endPosLocal;   // based on mount pos
-    Vec3 _endMountPosGlobal, _endMountPosLocal;
-    std::vector<Vec6> _theta;
-
-    /* inverse derivative kinematics */
-    RigidBodyDynamics::Math::MatrixNd _Jacobian;
-
-    /* inverse dynamics */
-    std::vector<RigidBodyDynamics::Math::SpatialVector> _endForceVec;
-
-    /* inverse kinematics */
-    bool _checkAngle(Vec6 angle);                   // check all angles once
-    Vec6 JointQMax;
-    Vec6 JointQMin;
-    Vec6 JointQdMax;
-    Vec6 JointQdMin;
-    double theta0[2], theta1[4], theta2[4], theta3[4], theta4[4], theta5[4];
-    double theta2Bias;
-
-    std::vector<Vec3> _linkPosLocal;    
-    std::vector<double> _disJoint;     
-    double _disP13;                   
-    Vec3 _vecP45;
-    Vec3 _vecP14;
-    Vec3 _vecP13;
-    Vec3 _vecP34;
-
-    Vec3 _joint4Axis;
-    Vec3 _joint5Axis;
-
-    Vec3 _plane324X, _plane324Z;        
-    double _vec34ProjX, _vec34ProjZ;
-    Vec3 _plane1234Y, _plane1234_X;     
-    Vec3 _vecP04ProjXY;                 
-
-    double _angle213, _angle123, _angle132;
-    double _angle_x13;
-
-    HomoMat _T46;
-
-    /* quadprog */
-    quadprogpp::Matrix<double> G, CE, CI;
-    quadprogpp::Vector<double> g0, ce0, ci0, x;
-
-    Mat6 _G;
-    Vec6 _g0;
-    VecX _ci0;
-    MatX _CI;
-};
-
-class Z1DynKineModel : public ArmDynKineModel{
-public:
-    Z1DynKineModel(Vec3 endPosLocal = Vec3::Zero(), 
-        double endEffectorMass = 0.0,
-        Vec3 endEffectorCom = Vec3::Zero(), 
-        Mat3 endEffectorInertia = Mat3::Zero());
-    ~Z1DynKineModel(){}
-
-};
-
-
-class Z1PlusDynKineModel : public ArmDynKineModel{
-public:
-    Z1PlusDynKineModel(Vec3 endPosLocal = Vec3::Zero(), 
-        double endEffectorMass = 0.0,
-        Vec3 endEffectorCom = Vec3::Zero(), 
-        Mat3 endEffectorInertia = Mat3::Zero());
-    ~Z1PlusDynKineModel(){}
-};
-
-#endif

include/model/ArmModel.h

@@ -0,0 +1,75 @@
+#ifndef ARMMODEL_H
+#define ARMMODEL_H
+
+#include <vector>
+#include "common/math/robotics.h"
+#include "common/math/quadProgpp/QuadProg++.hh"
+
+using namespace robo;
+
+class ArmModel{
+public:
+    ArmModel(Vec3 endPosLocal, double endEffectorMass, Vec3 endEffectorCom, Mat3 endEffectorInertia);
+    ~ArmModel(){};
+
+    HomoMat forwardKinematics(Vec6 q, int index = 6);
+    virtual bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false);
+    Mat6 CalcJacobian(Vec6 q);
+    Vec6 inverseDynamics(Vec6 q, Vec6 qd, Vec6 qdd, Vec6 Ftip);
+    virtual void solveQP(Vec6 twist, Vec6 qPast, Vec6& qd_result, double dt) = 0;
+
+    virtual bool checkInSingularity(Vec6 q) = 0;
+    void jointProtect(Vec6& q, Vec6& qd);
+
+    std::vector<double> getJointQMax() {return _jointQMax;}
+    std::vector<double> getJointQMin() {return _jointQMin;}
+    std::vector<double> getJointSpeedMax() {return _jointSpeedMax;}
+    void addLoad(double load);
+
+    const size_t dof = 6;
+protected:
+    bool _checkAngle(Vec6 );
+    void _buildModel();
+    // initial parameters
+    HomoMat _M; //End posture at the home position
+    std::vector<HomoMat> _Mlist;// List of link frames {i} relative to {i-1} at the home position
+    Vec3 _gravity;
+    Mat6 _Slist;// spatial twist at home position
+    std::vector<Mat6> _Glist;
+    std::vector<Vec3> _jointAxis;// omega
+    std::vector<Vec3> _jointPos;// p_0
+    std::vector<double> _jointQMax;
+    std::vector<double> _jointQMin;
+    std::vector<double> _jointSpeedMax;
+    Vec3 _endPosLocal; // based on mount postion
+    Vec3 _endMountPosGlobal;
+
+    std::vector<Vec3> _linkPosLocal;
+    std::vector<double> _disJoint;//distance between joint
+    std::vector<double> _linkMass;
+    std::vector<Vec3> _linkCom; // center of mass
+    std::vector<Mat3> _linkInertia;
+    double _endEffectorMass;
+    Vec3 _endEffectorCom;
+    Mat3 _endEffectorInertia;
+
+    double _load;
+};
+
+class Z1Model : public ArmModel{
+public:
+    Z1Model(Vec3 endPosLocal = Vec3::Zero(), double endEffectorMass = 0.0,
+            Vec3 endEffectorCom = Vec3::Zero(), Mat3 endEffectorInertia = Mat3::Zero());
+    ~Z1Model(){};
+
+    bool checkInSingularity(Vec6 q);
+    bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false);
+    void solveQP(Vec6 twist, Vec6 qPast, Vec6& qd_result, double dt);
+private:
+    void setParam_V3_6();
+    void setParam_V3_7();//long
+    void setParam_V3_8();
+    double _theta2Bias;
+};
+
+#endif

include/model/ArmReal.h

@@ -1,82 +0,0 @@
-#ifndef ARM_H
-#define ARM_H
-
-#include <vector>
-#include "common/math/mathTypes.h"
-#include "model/Joint.h"
-#include "model/Motor.h"
-#include "unitree_arm_sdk/udp.h"
-
-class ArmReal{
-public:
-    ArmReal(int dof, int motorNum, IOPort *ioPort);
-    virtual ~ArmReal();
-    void setJointKp(std::vector<double> jointKp);
-    void setJointKd(std::vector<double> jointKd);
-    void setJointQ(std::vector<double> jointQ);
-    void setJointDq(std::vector<double> jointDq);
-    void setJointTau(std::vector<double> jointTau);
-
-    void getJointQ(std::vector<double> &jointQState);
-    void getJointDq(std::vector<double> &jointDqState);
-    void getJointDDq(std::vector<double> &jointDDqState);
-    void getJointTau(std::vector<double> &jointTauState);
-
-    void getMotorQ(std::vector<double> &motorQ);
-    void getMotorDq(std::vector<double> &motorDq);
-    void getMotorTau(std::vector<double> &motorTau);
-
-    void armCalibration();
-    void armCalibration(std::vector<double> motorAngleBias);
-    bool armComm();
-    size_t getDof(){return _dof;}
-    size_t getMotorNum(){return _motorNum;}
-    // int  getMotorNum(){return _motorNum;}
-
-protected:
-    void _init();
-    void _getCmd(std::vector<MOTOR_send> &cmd);
-    void _setState(std::vector<MOTOR_recv> &motorState);
-    void _getMotorQBias();
-    // void _setComm();
-    void _setCaliBias();
-
-    int _motorNum;
-    int _dof;
-    int _commReturn;
-    bool _commYN, _motorCommYN;
-    std::vector<Joint*> _joints;
-    std::vector<double> _jointCaliAngle;
-    std::vector<double> _motorAngleBias;
-    std::vector<MOTOR_send> _motorCmd;
-    std::vector<MOTOR_recv> _motorState;
-
-    /* serial or UDP Communication */
-    IOPort *_ioPort;
-};
-
-class z1Real : public ArmReal{
-public:
-    z1Real(IOPort *ioPort);
-    ~z1Real(){}
-};
-
-class TigerHeadReal : public ArmReal{
-public:
-    TigerHeadReal(IOPort *ioPort);
-    ~TigerHeadReal(){}
-};
-
-class DogHeadReal : public ArmReal{
-public:
-    DogHeadReal(IOPort *ioPort);
-    ~DogHeadReal(){}
-};
-
-class TestSerial : public ArmReal{
-public:
-    TestSerial(IOPort *ioPort);
-    ~TestSerial(){}
-};
-
-#endif  // ARM_H

include/model/Joint.h

@@ -1,55 +0,0 @@
-#ifndef JOINT_H
-#define JOINT_H
-
-#include <vector>
-#include "model/Motor.h"
-#include "common/enumClass.h"
-
-class Joint{
-public:
-    Joint(){}
-    virtual ~Joint(){}
-    void setJointKp (double jointKp);
-    void setJointKd (double jointKd);
-    void setJointQ  (double jointQ);
-    void setJointDq (double jointDq);
-    void setJointTau(double jointTau);
-
-    double getJointQ();
-    double getJointDq();
-    double getJointDDq();
-    double getJointTau();
-
-    void getMotorQ(std::vector<double> &motorQ);
-    void getMotorDq(std::vector<double> &motorDq);
-    void getMotorDDq(std::vector<double> &motorDDq);
-    void getMotorTau(std::vector<double> &motorTau);
-
-    void getCmd(std::vector<MOTOR_send> &cmd);
-    void setState(std::vector<MOTOR_recv> &state);
-    void setCaliBias(double cali, std::vector<double> bias);
-
-protected:
-    JointMotorType _motorType;
-    int _motorNum;
-    double _jointInitAngle;
-    std::vector<Motor*> _motor;
-};
-
-class Jointz1 : public Joint{
-public:
-    Jointz1(int motorID, double direction);
-    Jointz1(int motorID0, int motorID1, double direction0, double direction1);
-    Jointz1(int motorID, int motorMsgOrder, double direction);
-    Jointz1(int motorID0, int motorID1, int motorMsgOrder0, int motorMsgOrder1, double direction0, double direction1);
-    ~Jointz1(){}
-};
-
-class JointTigerHead : public Joint{
-public:
-    JointTigerHead(int motorID, double direction);
-    JointTigerHead(int motorID, int motorMsgOrder, double direction);
-    ~JointTigerHead(){}
-};
-
-#endif  // JOINT_H

include/model/Motor.h

@@ -1,56 +0,0 @@
-#ifndef MOTOR_H
-#define MOTOR_H
-
-#include <vector>
-#include "common/enumClass.h"
-#include "unitree_arm_sdk/common/arm_motor_common.h"
-
-class Motor{
-public:
-    Motor(int id, int motorMsgOrder, double reductionRatio, MotorMountType type, double direction);
-    virtual ~Motor(){}
-
-    void setMotorKp (double jointKp);
-    void setMotorKd (double jointKd);
-    void setMotorQ  (double jointQ);
-    void setMotorDq (double jointDq);
-    void setMotorTau(double jointTau);
-
-    double getJointQ(){return _qJoint;}
-    double getJointDq(){return _dqJoint;}
-    double getJointDDq(){return _ddqJoint;}
-    double getJointTau(){return _tauJoint;}
-
-    void getMotorQ(std::vector<double> &motorQ);
-    void getMotorDq(std::vector<double> &motorDq);
-    void getMotorDDq(std::vector<double> &motorDDq);
-    void getMotorTau(std::vector<double> &motorTau);
-
-    void setQBias(std::vector<double> qBias);
-    void getCmd(std::vector<MOTOR_send> &cmd);
-    void setState(std::vector<MOTOR_recv> &state);
-protected:
-    int _id;
-    int _motorMsgOrder;
-    double _kp, _kd, _q, _dq, _tau;
-    double _qJoint, _dqJoint, _ddqJoint, _tauJoint;
-    double _qMotor, _dqMotor, _ddqMotor, _tauMotor;
-    double _reductionRatio;
-    double _directon;
-    double _qBias;
-    MotorMountType _type;
-};
-
-class Motorz1 : public Motor{
-public:
-    Motorz1(int id, int motorMsgOrder, MotorMountType type, double direction);
-    ~Motorz1(){}
-};
-
-class MotorGo1 : public Motor{
-public:
-    MotorGo1(int id, int motorMsgOrder, MotorMountType type, double direction);
-    ~MotorGo1(){}
-};
-
-#endif  // MOTOR_H

include/trajectory/CartesionSpaceTraj.h

@@ -1,45 +0,0 @@
-#ifndef CartesionSpaceTraj_H
-#define CartesionSpaceTraj_H
-
-#include <vector>
-#include <trajectory/Trajectory.h>
-#include <string>
-#include "control/CtrlComponents.h"
-
-class CartesionSpaceTraj : public Trajectory{
-public:
-    CartesionSpaceTraj(CtrlComponents *ctrlComp);
-    CartesionSpaceTraj(ArmDynKineModel *armModel);
-    CartesionSpaceTraj(ArmDynKineModel *armModel, CSVTool *csvState);
-    ~CartesionSpaceTraj(){}
-
-    void setCartesionTraj(Vec6 startP, Vec6 endP, double oriSpeed, double posSpeed);
-    void setCartesionTraj(Vec6 startP, Vec6 middleP, double middleS, Vec6 endP, double oriSpeed, double posSpeed);
-    bool getCartesionCmd(Vec6 pastPosture, Vec6 &endPosture, Vec6 &endTwist);
-
-    void setCartesionTrajMoveC(Vec6 startP, Vec6 middleP, Vec6 endP, double oriSpeed, double posSpeed);
-    bool getCartesionCmdMoveC(Vec6 pastPosture, Vec6 &endPosture, Vec6 &endTwist);
-
-    double _radius;
-
-private:
-    void _centerCircle(Vec3 p1, Vec3 p2, Vec3 p3);
-    bool _checkPostureValid(Vec6 endPosture, int pointOrder);
-    void _generateA345();
-
-    double _pathTimeTemp;       // path total time
-    double _s, _sDot;       // [0, 1]
-    double _a3, _a4, _a5;   // parameters of path
-
-    int _trajOrder;     // The order of trajectory curve
-    std::vector<Vec6> _curveParam;
-
-    /* MoveC instruct */
-    Vec3 _center;
-    double _theta;
-    Vec3 _axis_x;
-    Vec3 _axis_y;
-    Vec3 _axis_z;
-};
-
-#endif  // JOINTSPACETRAJ_H

include/trajectory/EndCircleTraj.h

@@ -1,13 +1,11 @@
 #ifndef ENDCIRCLETRAJ_H
 #define ENDCIRCLETRAJ_H
 
-#include "model/ArmDynKineModel.h"
 #include "trajectory/EndHomoTraj.h"
 
 class EndCircleTraj: public EndHomoTraj{
 public:
     EndCircleTraj(CtrlComponents *ctrlComp);
-    EndCircleTraj(ArmDynKineModel *armModel);
     ~EndCircleTraj(){}
 
     void setEndRoundTraj(HomoMat startHomo, Vec3 axisPointFromInit, 
@@ -16,21 +14,17 @@ public:
     void setEndRoundTraj(std::string stateName, Vec3 axisPointFromInit, 
             Vec3 axisDirection, double maxSpeed, double angle,
             bool keepOrientation = true);
-    void setEndRoundTraj(Vec6 startP, Vec6 middleP, Vec6 endP,
+    void setEndRoundTraj(Vec6 startP, Vec6 middleP, Vec6 endP, double speed);
-            double maxSpeed);
 private:
     void _centerCircle(Vec3 p1, Vec3 p2, Vec3 p3);
     bool _getEndTraj(HomoMat &homo, Vec6 &twist);
-    Vec3 _center;
+    Vec3 _center, _omegaAxis;
-    double _radius;
+    double _radius, _theta;
+
     Vec6 _middlePosture;
     Vec6 _middleQ;
-    HomoMat _middleHomo;
+    HomoMat _initHomoToCenter, _middleHomo, _centerHomo;
-    HomoMat _centerHomo;
-    HomoMat _initHomoToCenter;
     RotMat _initOri;
-    double _maxSpeed, _goalAngle, _speed, _angle;
-    Vec3 _omegaAxis;
 
     bool _keepOrientation;
 };

include/trajectory/EndHomoTraj.h

@@ -1,14 +1,13 @@
 #ifndef ENDHOMOTRAJ_H
 #define ENDHOMOTRAJ_H
 
-#include "model/ArmDynKineModel.h"
+#include "model/ArmModel.h"
 #include "trajectory/Trajectory.h"
 #include "trajectory/SCurve.h"
 
 class EndHomoTraj : public Trajectory{
 public:
     EndHomoTraj(CtrlComponents *ctrlComp);
-    EndHomoTraj(ArmDynKineModel *armModel);
     virtual ~EndHomoTraj();
     bool getJointCmd(Vec6 &q, Vec6 &qd);
     bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd);
@@ -19,16 +18,7 @@ protected:
     HomoMat _cmdHomo;
     Vec6 _cmdTwist;
 
-    Vec6 _deltaPosture;
+    SCurve _sCurve;
-    Vec3 _omgtheta;
-    double _theta;
-    Mat3 _startR, _endR, _delatR, _currentR;
-    Vec3 _startp, _endp, _currentp;
-    SCurve *_sCurve;
-private:
-    bool checkInSingularity();
-    void _checkAngleValid(const Vec6 &q, int pointOrder);
-    bool _checkJointAngleValid(const double &q, int jointOrder);
 };
 
 #endif  // ENDHOMOTRAJ_H

include/trajectory/EndLineTraj.h

@@ -7,24 +7,18 @@
 class EndLineTraj : public EndHomoTraj{
 public:
     EndLineTraj(CtrlComponents *ctrlComp);
-    EndLineTraj(ArmDynKineModel *armModel);
     ~EndLineTraj(){}
 
-    void setEndLineTraj(HomoMat startHomo, Vec3 deltaPos, Vec3 deltaOri, double maxMovingSpeed, double maxTurningSpeed);
+    bool setEndLineTraj(Vec6 startP, Vec6 endP, double speed);
-    void setEndLineTraj(std::string stateName, Vec3 deltaPos, Vec3 deltaOri, double maxMovingSpeed, double maxTurningSpeed);
+    bool setEndLineTraj(HomoMat startHomo, Vec3 deltaPos, Vec3 deltaOri, double speed);
-    void setEndLineTraj(Vec6 startP, Vec6 endP, double maxMovingSpeed, double maxTurningSpeed);
+    bool setEndLineTraj(std::string stateName, Vec3 deltaPos, Vec3 deltaOri, double speed);
-    void setEndLineTraj(std::string startName, std::string endName, double maxMovingSpeed, double maxTurningSpeed);
+    bool setEndLineTraj(std::string startName, std::string endName, double speed);
 private:
     bool _getEndTraj(HomoMat &homo, Vec6 &twist);
 
-    // SCurve _sCurves[2];    // 0: position, 1: orientation
+    Vec3 _currentDistance, _currentAngle, _omgtheta;
-    Vec3 _movingAxis, _turningAxis;
+    Mat3 _startR, _endR, _delatR, _currentR;
-    double _movingDistance, _turningAngle;
+    Vec3 _startp, _endp, _deltap, _currentp;
-    Vec3 _currentDistance, _currentAngle;
-    Vec3 _currentVelocity, _currentOmega;
-       
-    SCurve _posCurve;
-    SCurve _oriCurve;
 };
 
 #endif

include/trajectory/JointSpaceTraj.h

@@ -10,36 +10,19 @@
 class JointSpaceTraj : public Trajectory{
 public:
     JointSpaceTraj(CtrlComponents *ctrlComp);
-    JointSpaceTraj(ArmDynKineModel *armModel);
-    JointSpaceTraj(ArmDynKineModel *armModel, CSVTool *csvState);
     ~JointSpaceTraj(){}
 
-    void setGripper(double startQ, double endQ);
     bool getJointCmd(Vec6 &q, Vec6 &qd);
     bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd);
+    
+    void setGripper(double startQ, double endQ, double speed = M_PI);
     void setJointTraj(Vec6 startQ, Vec6 endQ, double speed);
     bool setJointTraj(Vec6 startQ, std::string endName, double speed);
-    void setJointTraj(std::string startName, std::string endName, double speed);
+    bool setJointTraj(std::string startName, std::string endName, double speed);
-
-    void setJointTraj(Vec6 startQ, Vec6 middleQ, double middleS, Vec6 endQ, double speed);
-    void setJointTraj(std::string startName, std::string middleName, double middleS, std::string endName, double speed);
-    void setJointTraj(std::vector<Vec6> stateQ, std::vector<double> stateS, double pathTime);
-    void setJointTraj(std::vector<std::string> stateName, std::vector<double> stateS, double pathTime);
-
 private:
-    void _checkAngleValid(const Vec6 &q, int pointOrder);
-    bool _checkJointAngleValid(const double &q, int jointOrder);
-    void _generateA345();
-
     SCurve _jointCurve;
-    int _jointNum;
+    double ddQMax;
-    double _pathTimeTemp;       // path total time
+    double dddQMax;
-    double _s, _sDot;       // [0, 1]
-    double _a3, _a4, _a5;   // parameters of path
-
-    int _trajOrder;     // The order of trajectory curve
-    std::vector<Vec6> _curveParam;
-
 };
 
 #endif  // JOINTSPACETRAJ_H

include/trajectory/SCurve.h

@@ -3,7 +3,6 @@
 
 /* 归一化后的s曲线 */
 
-#include <math.h>
 #include <iostream>
 
 class SCurve{

include/trajectory/StopForTime.h

@@ -6,19 +6,12 @@
 class StopForTime : public Trajectory{
 public:
     StopForTime(CtrlComponents *ctrlComp);
-    StopForTime(ArmDynKineModel *armModel);
-    StopForTime(ArmDynKineModel *armModel, CSVTool *csvState);
-
     ~StopForTime(){}
 
     bool getJointCmd(Vec6 &q, Vec6 &qd);
     bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd);
-
     void setStop(Vec6 stopQ, double stopTime);
-    void setStop(std::string stateName, double stopTime);
 private:
-    // Vec6 _stopQ;
-    // double _stopTime;
 };
 
 #endif  // STOPFORTIME_H

include/trajectory/Trajectory.h

@@ -1,124 +1,47 @@
 #ifndef TRAJECTORY_H
 #define TRAJECTORY_H
 
-#include "common/math/mathTypes.h"
+#include "common/math/mathTools.h"
 #include "control/CtrlComponents.h"
-#include "unitree_arm_sdk/timer.h"
+#include "common/utilities/timer.h"
 
 class Trajectory{
 public:
-    Trajectory(CtrlComponents *ctrlComp){
+    Trajectory(CtrlComponents *ctrlComp);
-        _ctrlComp = ctrlComp;
-        _armModel = ctrlComp->armModel;
-        _csvState = ctrlComp->stateCSV;
-        _dof = _armModel->getDOF();
-        _hasKinematic = true;
-        _jointMaxQ = _armModel->getJointQMax();
-        _jointMinQ = _armModel->getJointQMin();
-        _jointMaxSpeed = _armModel->getJointSpeedMax();
-    }
-    Trajectory(ArmDynKineModel *armModel){
-        _ctrlComp = nullptr;
-        _armModel = armModel;
-        _csvState = nullptr;
-        _dof = armModel->getDOF();
-        if(_dof == 6){
-            _hasKinematic = true;
-        }
-        _jointMaxQ = _armModel->getJointQMax();
-        _jointMinQ = _armModel->getJointQMin();
-        _jointMaxSpeed = _armModel->getJointSpeedMax();
-    }
-    Trajectory(ArmDynKineModel *armModel, CSVTool *csvState):Trajectory(armModel){
-        _csvState = csvState;
-    }
     virtual ~Trajectory(){}
     virtual bool getJointCmd(Vec6 &q, Vec6 &qd){return false;};
     virtual bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd){return false;};
     virtual bool getCartesionCmd(Vec6 pastPosture, Vec6 &endPosture, Vec6 &endTwist){return false;};
 
-    void restart(){
+    void restart();
-        _pathStarted = false;
+    virtual void setGripper(double startQ, double endQ, double speed = M_PI);
-        _reached = false;
+
-        _qPast = _startQ;
-    }
-    virtual void setGripper(double startQ, double endQ){
-        _gripperStartQ = startQ;
-        _gripperEndQ   = endQ;
-    }
     bool correctYN(){return _settingCorrect;}
     Vec6 getStartQ(){return _startQ;}
     Vec6 getEndQ(){return _endQ;}
-    double getEndGripperQ(){return _gripperEndQ;};
+    double getEndGripperQ(){return _endGripperQ;};
-    double getStartGripperQ(){return _gripperStartQ;};
+    double getStartGripperQ(){return _startGripperQ;};
-    HomoMat getStartHomo(){
+    HomoMat getStartHomo(){return _startHomo;};
-        if(_hasKinematic){
+    HomoMat getEndHomo(){return _endHomo;};
-            return _startHomo;
+    Vec6 getEndPosture(){return _endPosture;};
-        }else{
-            std::cout << "[ERROR] This trajectory do not have kinematics" << std::endl;
-            exit(-1);
-        }
-    }
-    HomoMat getEndHomo(){
-        if(_hasKinematic){
-            return _endHomo;
-        }else{
-            std::cout << "[ERROR] This trajectory do not have kinematics" << std::endl;
-            exit(-1);
-        }
-    }
-
-    Vec6 getEndPosture(){
-        if(_hasKinematic){
-            return _endPosture;
-        }else{
-            std::cout << "[ERROR] This trajectory do not have kinematics" << std::endl;
-            exit(-1);
-        }
-    }
     double getPathTime(){return _pathTime;}
 protected:
+    void _runTime();
+
     CtrlComponents *_ctrlComp;
-    ArmDynKineModel *_armModel;
+    ArmModel *_armModel;
     CSVTool *_csvState;
     bool _pathStarted = false;
     bool _reached = false;
-    bool _paused = false;
     bool _settingCorrect = true;
-    double _startTime;
+    double _startTime, _currentTime, _pathTime, _tCost;
-    double _currentTime;
-    double _pathTime;
-    double _tCost;
 
     Vec6 _qPast;
-    Vec6 _startPosture, _endPosture;
+    Vec6 _startQ, _endQ, _deltaQ;
-    Vec6 _startQ, _endQ;
     HomoMat _startHomo, _endHomo;
+    Vec6 _startPosture, _endPosture, _deltaPosture;
+    double _startGripperQ, _endGripperQ;
 
-    double _gripperStartQ;
-    double _gripperEndQ;
-
-    size_t _dof;
-    bool _hasKinematic;
-
-    std::vector<double> _jointMaxQ;
-    std::vector<double> _jointMinQ;
-    std::vector<double> _jointMaxSpeed;
-
-    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;
-    }
 };
 
 #endif  // TRAJECTORY_H

include/trajectory/TrajectoryManager.h

@@ -11,8 +11,6 @@
 class TrajectoryManager{
 public:
     TrajectoryManager(CtrlComponents *ctrlComp);
-    TrajectoryManager(ArmDynKineModel *armModel);
-    TrajectoryManager(ArmDynKineModel *armModel, CSVTool *csvState);
     ~TrajectoryManager(){}
     bool getJointCmd(Vec6 &q, Vec6 &qd);
     bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd);
@@ -23,10 +21,10 @@ public:
     Vec6 getStartQ();
     Vec6 getEndQ();
     Vec6 getEndPosture();
-    double getEndGripperQ();
     double getStartGripperQ();
+    double getEndGripperQ();
     HomoMat getEndHomo();
-    // bool checkTrajectoryContinuous();
+    size_t size() {return _trajVec.size();} ;
 private:
     CtrlComponents *_ctrlComp;
     JointSpaceTraj *_trajBack;

main.cpp

@@ -1,38 +1,29 @@
-#include <iostream>
-#include <unistd.h>
 #include <csignal>
 #include <sched.h>
-#include <iomanip>
+#include "FSM/FiniteStateMachine.h"
-#include "control/CtrlComponents.h"
+#include "FSM/State_Passive.h"
-#include "model/ArmDynKineModel.h"
-#include "interface/IOUDP.h"
-
 #include "FSM/State_BackToStart.h"
+#include "FSM/State_Calibration.h"
 #include "FSM/State_Cartesian.h"
+#include "FSM/State_JointSpace.h"
 #include "FSM/State_MoveJ.h"
 #include "FSM/State_MoveL.h"
 #include "FSM/State_MoveC.h"
-#include "FSM/State_Dance.h"
+#include "FSM/State_ToState.h"
-#include "FSM/State_JointSpace.h"
-#include "FSM/State_Passive.h"
 #include "FSM/State_SaveState.h"
 #include "FSM/State_Teach.h"
 #include "FSM/State_TeachRepeat.h"
-#include "FSM/State_ToState.h"
 #include "FSM/State_Trajectory.h"
-#include "FSM/State_Calibration.h"
 #include "FSM/State_LowCmd.h"
-#include "FSM/FiniteStateMachine.h"
 #include "FSM/State_SetTraj.h"
 
-#include "unitree_arm_sdk/unitree_arm_sdk.h"
+const std::string Z1_CTRL_VERSION = "2022.11.11";
-
 bool running = true;
 
 // over watch the ctrl+c command
 void ShutDown(int sig){
-	std::cout << "[STATE] stop the controller" << std::endl;
     running = false;
+	std::cout << "[STATE] stop the controller" << std::endl;
 }
 
 //set real-time program
@@ -46,105 +37,88 @@ void setProcessScheduler(){
 }
 
 int main(int argc, char **argv){
+    if(argc == 2) {
+        if((strcmp(argv[1], "-v") == 0) || (strcmp(argv[1], "--version") == 0)){
+            std::cout << "Version z1_controller: "<< Z1_CTRL_VERSION<<std::endl;
+            return 0;
+        }
+    }
+    
     /* set real-time process */
     setProcessScheduler();
     /* set the print format */
     std::cout << std::fixed << std::setprecision(3);
 
+
     EmptyAction emptyAction((int)ArmFSMStateName::INVALID);
     std::vector<KeyAction*> events;
-    CmdPanel *cmdPanel;
     CtrlComponents *ctrlComp = new CtrlComponents();
 
-    if(ctrlComp->ctrl == Control::_SDK)
+    // control method
-    {
+    if(argc == 2) {
-        events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART));
+        if(argv[1][0] == 'k'){
-        events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE));
+            ctrlComp->ctrl = Control::KEYBOARD;
-        events.push_back(new StateAction("2", (int)ArmFSMStateName::JOINTCTRL));
+        }else if(argv[1][0] == 's'){
-        events.push_back(new StateAction("3", (int)ArmFSMStateName::CARTESIAN));
+            ctrlComp->ctrl = Control::SDK;
-        events.push_back(new StateAction("4", (int)ArmFSMStateName::MOVEJ));
+        }else if(argv[1][0] == 'j'){
-        events.push_back(new StateAction("5", (int)ArmFSMStateName::MOVEL));
+            ctrlComp->ctrl = Control::JOYSTICK;
-        events.push_back(new StateAction("6", (int)ArmFSMStateName::MOVEC));
+        }
-        events.push_back(new StateAction("7", (int)ArmFSMStateName::TEACH));
-        events.push_back(new StateAction("8", (int)ArmFSMStateName::TEACHREPEAT));
-        events.push_back(new StateAction("9", (int)ArmFSMStateName::SAVESTATE));
-        events.push_back(new StateAction("0", (int)ArmFSMStateName::TOSTATE));
-        events.push_back(new StateAction("-", (int)ArmFSMStateName::TRAJECTORY));
-        events.push_back(new StateAction("=", (int)ArmFSMStateName::CALIBRATION));
-        events.push_back(new StateAction("]", (int)ArmFSMStateName::NEXT));
-        events.push_back(new StateAction("/", (int)ArmFSMStateName::LOWCMD));
-        events.push_back(new StateAction("l", (int)ArmFSMStateName::SETTRAJ));
-
-        events.push_back(new ValueAction("q", "a", 1.0));
-        events.push_back(new ValueAction("w", "s", 1.0));
-        events.push_back(new ValueAction("e", "d", 1.0));
-        events.push_back(new ValueAction("r", "f", 1.0));
-        events.push_back(new ValueAction("t", "g", 1.0));
-        events.push_back(new ValueAction("y", "h", 1.0));
-        events.push_back(new ValueAction("down", "up", 1.0));
-        
-        cmdPanel = new UnitreeKeyboardUDPRecv(events, emptyAction);
-    }else if(ctrlComp->ctrl == Control::_KEYBOARD){
-        events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART));
-        events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE));
-        events.push_back(new StateAction("2", (int)ArmFSMStateName::JOINTCTRL));
-        events.push_back(new StateAction("3", (int)ArmFSMStateName::CARTESIAN));
-        events.push_back(new StateAction("4", (int)ArmFSMStateName::MOVEJ));
-        events.push_back(new StateAction("5", (int)ArmFSMStateName::MOVEL));
-        events.push_back(new StateAction("6", (int)ArmFSMStateName::MOVEC));
-        events.push_back(new StateAction("7", (int)ArmFSMStateName::TEACH));
-        events.push_back(new StateAction("8", (int)ArmFSMStateName::TEACHREPEAT));
-        events.push_back(new StateAction("9", (int)ArmFSMStateName::SAVESTATE));
-        events.push_back(new StateAction("0", (int)ArmFSMStateName::TOSTATE));
-        events.push_back(new StateAction("-", (int)ArmFSMStateName::TRAJECTORY));
-        events.push_back(new StateAction("=", (int)ArmFSMStateName::CALIBRATION));
-        events.push_back(new StateAction("]", (int)ArmFSMStateName::NEXT));
-
-        events.push_back(new ValueAction("q", "a", 1.0));
-        events.push_back(new ValueAction("w", "s", 1.0));
-        events.push_back(new ValueAction("e", "d", 1.0));
-        events.push_back(new ValueAction("r", "f", 1.0));
-        events.push_back(new ValueAction("t", "g", 1.0));
-        events.push_back(new ValueAction("y", "h", 1.0));
-        events.push_back(new ValueAction("down", "up", 1.0));
-        
-        cmdPanel = new Keyboard(events, emptyAction);
-    }else if(ctrlComp->ctrl == Control::_JOYSTICK){
-        events.push_back(new StateAction("r2x", (int)ArmFSMStateName::TRAJECTORY));
-        events.push_back(new StateAction("l12", (int)ArmFSMStateName::PASSIVE));
-        events.push_back(new StateAction("r2", (int)ArmFSMStateName::JOINTCTRL));
-        events.push_back(new StateAction("r1", (int)ArmFSMStateName::CARTESIAN));
-        events.push_back(new StateAction("select", (int)ArmFSMStateName::BACKTOSTART));
-
-        events.push_back(new ValueAction("left_up", "left_down", 1.0));//Tran_Y
-        events.push_back(new ValueAction("left_left", "left_right", -1.0));//Tran_X, inverse
-        events.push_back(new ValueAction("up", "down", 1.0));//Tran_Z
-        events.push_back(new ValueAction("right_up", "right_down", 1.0));//Rot_Y
-        events.push_back(new ValueAction("right_left", "right_right", 1.0));//Rot_x
-        events.push_back(new ValueAction("Y", "A", 1.0));//Rot_Z
-        events.push_back(new ValueAction("right", "left",  1.0));//girpper, close-open
-
-        cmdPanel = new UnitreeJoystick(events, emptyAction);
     }
     
-#ifdef RUN_ROS
+#ifdef COMPILE_WITH_SIMULATION
     ros::init(argc, argv, "z1_controller");
-#endif  // RUN_ROS
+#endif
 
-    ctrlComp->dof = 6;
+    // ctrlComp->isPlot = true;
+    ctrlComp->dt = 1.0/250.;
     ctrlComp->armConfigPath =  "../config/";
     ctrlComp->stateCSV = new CSVTool("../config/savedArmStates.csv");
-    ctrlComp->running = &running;
-#ifdef UDP
-    ctrlComp->ioInter = new IOUDP(cmdPanel, ctrlComp->ctrl_IP.c_str(), ctrlComp->_hasGripper);
-#elif defined ROS
-    ctrlComp->ioInter = new IOROS(cmdPanel, ctrlComp->_hasGripper);
-#endif
-    ctrlComp->dt = 0.004;
     ctrlComp->geneObj();
+    if(ctrlComp->ctrl == Control::SDK){
+        ctrlComp->cmdPanel = new ARMSDK(events, emptyAction, "127.0.0.1", 8072, 0.002);
+    }else if(ctrlComp->ctrl == Control::KEYBOARD){
+        events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART));
+        events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE));
+        events.push_back(new StateAction("2", (int)ArmFSMStateName::JOINTCTRL));
+        events.push_back(new StateAction("3", (int)ArmFSMStateName::CARTESIAN));
+        events.push_back(new StateAction("4", (int)ArmFSMStateName::MOVEJ));
+        events.push_back(new StateAction("5", (int)ArmFSMStateName::MOVEL));
+        events.push_back(new StateAction("6", (int)ArmFSMStateName::MOVEC));
+        events.push_back(new StateAction("7", (int)ArmFSMStateName::TEACH));
+        events.push_back(new StateAction("8", (int)ArmFSMStateName::TEACHREPEAT));
+        events.push_back(new StateAction("9", (int)ArmFSMStateName::SAVESTATE));
+        events.push_back(new StateAction("0", (int)ArmFSMStateName::TOSTATE));
+        events.push_back(new StateAction("-", (int)ArmFSMStateName::TRAJECTORY));
+        events.push_back(new StateAction("=", (int)ArmFSMStateName::CALIBRATION));
 
-    std::vector<BaseState*> states;
+        events.push_back(new ValueAction("q", "a", 0.5));
-    states.push_back(new State_Passive(ctrlComp));      // First state in states is the begining state for FSM
+        events.push_back(new ValueAction("w", "s", 0.5));
+        events.push_back(new ValueAction("e", "d", 0.5));
+        events.push_back(new ValueAction("r", "f", 0.5));
+        events.push_back(new ValueAction("t", "g", 0.5));
+        events.push_back(new ValueAction("y", "h", 0.5));
+        events.push_back(new ValueAction("down", "up", 1.));
+
+        ctrlComp->cmdPanel = new Keyboard(events, emptyAction);
+    }else if(ctrlComp->ctrl == Control::JOYSTICK){
+        events.push_back(new StateAction("r2x",     (int)ArmFSMStateName::TRAJECTORY));
+        events.push_back(new StateAction("l12",     (int)ArmFSMStateName::PASSIVE));
+        events.push_back(new StateAction("r2",      (int)ArmFSMStateName::JOINTCTRL));
+        events.push_back(new StateAction("r1",      (int)ArmFSMStateName::CARTESIAN));
+        events.push_back(new StateAction("select",  (int)ArmFSMStateName::BACKTOSTART));
+
+        events.push_back(new ValueAction("left_up",     "left_down",    0.5));//Tran_Y
+        events.push_back(new ValueAction("left_right",   "left_left",   0.5));//Tran_X, inverse
+        events.push_back(new ValueAction("up",          "down",         -0.5));//Tran_Z, inverse
+        events.push_back(new ValueAction("right_up",    "right_down",   0.5));//Rot_Y
+        events.push_back(new ValueAction("right_left",  "right_right",  0.5));//Rot_x
+        events.push_back(new ValueAction("Y",           "A",            0.5));//Rot_Z
+        events.push_back(new ValueAction("right",       "left",         1.0));//girpper, close-open
+
+        ctrlComp->cmdPanel = new UnitreeJoystick(events, emptyAction);
+    }
+    std::vector<FSMState*> states;
+    states.push_back(new State_Passive(ctrlComp));
     states.push_back(new State_BackToStart(ctrlComp));
     states.push_back(new State_JointSpace(ctrlComp));
     states.push_back(new State_Cartesian(ctrlComp));
@@ -160,19 +134,16 @@ int main(int argc, char **argv){
     states.push_back(new State_Calibration(ctrlComp));
     states.push_back(new State_SetTraj(ctrlComp));
 
-    FiniteStateMachine fsm(states, cmdPanel, 0, ctrlComp->dt);
+    FiniteStateMachine *fsm;
+    fsm = new FiniteStateMachine(states, ctrlComp);
 
+    ctrlComp->running = &running;
     signal(SIGINT, ShutDown);
-
     while(running){
         usleep(100000);
     }
 
-#ifdef COMPILE_DEBUG
+    delete fsm;
-    ctrlComp->writeData();
-#endif
-
     delete ctrlComp;
-
     return 0;
 }

package.xml

@@ -10,15 +10,12 @@
 
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>roscp</build_depend>
-  <build_depend>rospy</build_depend>
   <build_depend>urdf</build_depend>
   <build_depend>xacro</build_depend>
   <build_export_depend>roscp</build_export_depend>
-  <build_export_depend>rospy</build_export_depend>
   <build_export_depend>urdf</build_export_depend>
   <build_export_depend>xacro</build_export_depend>
   <exec_depend>roscp</exec_depend>
-  <exec_depend>rospy</exec_depend>
   <exec_depend>urdf</exec_depend>
   <exec_depend>xacro</exec_depend>
 

thirdparty/quadProgpp/src/Array.cc

@@ -1,33 +0,0 @@
-// $Id: Array.cc 201 2008-05-18 19:47:38Z digasper $
-// This file is part of QuadProg++:  
-// Copyright (C) 2006--2009 Luca Di Gaspero. 
-//
-// This software may be modified and distributed under the terms
-// of the MIT license.  See the LICENSE file for details.
-
-#include "quadProgpp/include/Array.hh"
-
-/**
-  Index utilities
- */
-
-namespace quadprogpp {
-
-std::set<unsigned int> seq(unsigned int s, unsigned int e)
-{
-	std::set<unsigned int> tmp;
-	for (unsigned int i = s; i <= e; i++)
-		tmp.insert(i);
-	
-	return tmp;
-}
-
-std::set<unsigned int> singleton(unsigned int i)
-{
-	std::set<unsigned int> tmp;
-	tmp.insert(i);
-	
-	return tmp;
-}
-
-}  // namespace quadprogpp

thirdparty/quadProgpp/src/QuadProg++.cc

@@ -1,790 +0,0 @@
-/* 
-File $Id: QuadProg++.cc 232 2007-06-21 12:29:00Z digasper $
- 
- Author: Luca Di Gaspero
- DIEGM - University of Udine, Italy
- luca.digaspero@uniud.it
- http://www.diegm.uniud.it/digaspero/
- 
- This software may be modified and distributed under the terms
- of the MIT license.  See the LICENSE file for details.
- 
- */
-
-#include <iostream>
-#include <algorithm>
-#include <cmath>
-#include <limits>
-#include <sstream>
-#include <stdexcept>
-#include "quadProgpp/include/QuadProg++.hh"
-//#define TRACE_SOLVER
-
-namespace quadprogpp {
-
-// Utility functions for updating some data needed by the solution method 
-void compute_d(Vector<double>& d, const Matrix<double>& J, const Vector<double>& np);
-void update_z(Vector<double>& z, const Matrix<double>& J, const Vector<double>& d, int iq);
-void update_r(const Matrix<double>& R, Vector<double>& r, const Vector<double>& d, int iq);
-bool add_constraint(Matrix<double>& R, Matrix<double>& J, Vector<double>& d, unsigned int& iq, double& rnorm);
-void delete_constraint(Matrix<double>& R, Matrix<double>& J, Vector<int>& A, Vector<double>& u, unsigned int n, int p, unsigned int& iq, int l);
-
-// Utility functions for computing the Cholesky decomposition and solving
-// linear systems
-void cholesky_decomposition(Matrix<double>& A);
-void cholesky_solve(const Matrix<double>& L, Vector<double>& x, const Vector<double>& b);
-void forward_elimination(const Matrix<double>& L, Vector<double>& y, const Vector<double>& b);
-void backward_elimination(const Matrix<double>& U, Vector<double>& x, const Vector<double>& y);
-
-// Utility functions for computing the scalar product and the euclidean 
-// distance between two numbers
-double scalar_product(const Vector<double>& x, const Vector<double>& y);
-double distance(double a, double b);
-
-// Utility functions for printing vectors and matrices
-void print_matrix(const char* name, const Matrix<double>& A, int n = -1, int m = -1);
-
-template<typename T>
-void print_vector(const char* name, const Vector<T>& v, int n = -1);
-
-// The Solving function, implementing the Goldfarb-Idnani method
-double solve_quadprog(Matrix<double>& G, Vector<double>& g0, 
-                      const Matrix<double>& CE, const Vector<double>& ce0,  
-                      const Matrix<double>& CI, const Vector<double>& ci0, 
-                      Vector<double>& x)
-{
-  std::ostringstream msg;
-  unsigned int n = G.ncols(), p = CE.ncols(), m = CI.ncols();
-  if (G.nrows() != n)
-  {
-    msg << "The matrix G is not a squared matrix (" << G.nrows() << " x " << G.ncols() << ")";
-    throw std::logic_error(msg.str());
-  }
-  if (CE.nrows() != n)
-  {
-    msg << "The matrix CE is incompatible (incorrect number of rows " << CE.nrows() << " , expecting " << n << ")";
-    throw std::logic_error(msg.str());
-  }
-  if (ce0.size() != p)
-  {
-    msg << "The vector ce0 is incompatible (incorrect dimension " << ce0.size() << ", expecting " << p << ")";
-    throw std::logic_error(msg.str());
-  }
-  if (CI.nrows() != n)
-  {
-    msg << "The matrix CI is incompatible (incorrect number of rows " << CI.nrows() << " , expecting " << n << ")";
-    throw std::logic_error(msg.str());
-  }
-  if (ci0.size() != m)
-  {
-    msg << "The vector ci0 is incompatible (incorrect dimension " << ci0.size() << ", expecting " << m << ")";
-    throw std::logic_error(msg.str());
-  }
-  x.resize(n);
-  register unsigned int i, j, k, l; /* indices */
-  int ip; // this is the index of the constraint to be added to the active set
-  Matrix<double> R(n, n), J(n, n);
-  Vector<double> s(m + p), z(n), r(m + p), d(n), np(n), u(m + p), x_old(n), u_old(m + p);
-  double f_value, psi, c1, c2, sum, ss, R_norm;
-  double inf;
-  if (std::numeric_limits<double>::has_infinity)
-    inf = std::numeric_limits<double>::infinity();
-  else
-    inf = 1.0E300;
-  double t, t1, t2; /* t is the step lenght, which is the minimum of the partial step length t1 
-    * and the full step length t2 */
-  Vector<int> A(m + p), A_old(m + p), iai(m + p);
-  unsigned int iq, iter = 0;
-  Vector<bool> iaexcl(m + p);
-	
-  /* p is the number of equality constraints */
-  /* m is the number of inequality constraints */
-#ifdef TRACE_SOLVER
-  std::cout << std::endl << "Starting solve_quadprog" << std::endl;
-  print_matrix("G", G);
-  print_vector("g0", g0);
-  print_matrix("CE", CE);
-  print_vector("ce0", ce0);
-  print_matrix("CI", CI);
-  print_vector("ci0", ci0);
-#endif  
-  
-  /*
-   * Preprocessing phase
-   */
-	
-  /* compute the trace of the original matrix G */
-  c1 = 0.0;
-  for (i = 0; i < n; i++)
-  {
-    c1 += G[i][i];
-  }
-  /* decompose the matrix G in the form L^T L */
-  cholesky_decomposition(G);
-#ifdef TRACE_SOLVER
-  print_matrix("G", G);
-#endif
-  /* initialize the matrix R */
-  for (i = 0; i < n; i++)
-  {
-    d[i] = 0.0;
-    for (j = 0; j < n; j++)
-      R[i][j] = 0.0;
-  }
-  R_norm = 1.0; /* this variable will hold the norm of the matrix R */
-  
-  /* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
-  c2 = 0.0;
-  for (i = 0; i < n; i++) 
-  {
-    d[i] = 1.0;
-    forward_elimination(G, z, d);
-    for (j = 0; j < n; j++)
-      J[i][j] = z[j];
-    c2 += z[i];
-    d[i] = 0.0;
-  }
-#ifdef TRACE_SOLVER
-  print_matrix("J", J);
-#endif
-  
-  /* c1 * c2 is an estimate for cond(G) */
-  
-  /* 
-    * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x 
-   * this is a feasible point in the dual space
-   * x = G^-1 * g0
-   */
-  cholesky_solve(G, x, g0);
-  for (i = 0; i < n; i++)
-    x[i] = -x[i];
-  /* and compute the current solution value */ 
-  f_value = 0.5 * scalar_product(g0, x);
-#ifdef TRACE_SOLVER
-  std::cout << "Unconstrained solution: " << f_value << std::endl;
-  print_vector("x", x);
-#endif
-  
-  /* Add equality constraints to the working set A */
-  iq = 0;
-  for (i = 0; i < p; i++)
-  {
-    for (j = 0; j < n; j++)
-      np[j] = CE[j][i];
-    compute_d(d, J, np);
-    update_z(z, J, d, iq);
-    update_r(R, r, d, iq);
-#ifdef TRACE_SOLVER
-    print_matrix("R", R, n, iq);
-    print_vector("z", z);
-    print_vector("r", r, iq);
-    print_vector("d", d);
-#endif
-    
-    /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint 
-      becomes feasible */
-    t2 = 0.0;
-    if (fabs(scalar_product(z, z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
-      t2 = (-scalar_product(np, x) - ce0[i]) / scalar_product(z, np);
-    
-    /* set x = x + t2 * z */
-    for (k = 0; k < n; k++)
-      x[k] += t2 * z[k];
-    
-    /* set u = u+ */
-    u[iq] = t2;
-    for (k = 0; k < iq; k++)
-      u[k] -= t2 * r[k];
-    
-    /* compute the new solution value */
-    f_value += 0.5 * (t2 * t2) * scalar_product(z, np);
-    A[i] = -i - 1;
-    
-    if (!add_constraint(R, J, d, iq, R_norm))
-    {	  
-      // Equality constraints are linearly dependent
-      throw std::runtime_error("Constraints are linearly dependent");
-      return f_value;
-    }
-  }
-  
-  /* set iai = K \ A */
-  for (i = 0; i < m; i++)
-    iai[i] = i;
-  
-l1:	iter++;
-#ifdef TRACE_SOLVER
-  print_vector("x", x);
-#endif
-  /* step 1: choose a violated constraint */
-  for (i = p; i < iq; i++)
-  {
-    ip = A[i];
-    iai[ip] = -1;
-  }
-	
-  /* compute s[x] = ci^T * x + ci0 for all elements of K \ A */
-  ss = 0.0;
-  psi = 0.0; /* this value will contain the sum of all infeasibilities */
-  ip = 0; /* ip will be the index of the chosen violated constraint */
-  for (i = 0; i < m; i++)
-  {
-    iaexcl[i] = true;
-    sum = 0.0;
-    for (j = 0; j < n; j++)
-      sum += CI[j][i] * x[j];
-    sum += ci0[i];
-    s[i] = sum;
-    psi += std::min(0.0, sum);
-  }
-#ifdef TRACE_SOLVER
-  print_vector("s", s, m);
-#endif
-  
-  
-  if (fabs(psi) <= m * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
-  {
-    /* numerically there are not infeasibilities anymore */
-    return f_value;
-  }
-  
-  /* save old values for u and A */
-  for (i = 0; i < iq; i++)
-  {
-    u_old[i] = u[i];
-    A_old[i] = A[i];
-  }
-  /* and for x */
-  for (i = 0; i < n; i++)
-    x_old[i] = x[i];
-  
-l2: /* Step 2: check for feasibility and determine a new S-pair */
-    for (i = 0; i < m; i++)
-    {
-      if (s[i] < ss && iai[i] != -1 && iaexcl[i])
-      {
-        ss = s[i];
-        ip = i;
-      }
-    }
-  if (ss >= 0.0)
-  {
-    return f_value;
-  }
-  
-  /* set np = n[ip] */
-  for (i = 0; i < n; i++)
-    np[i] = CI[i][ip];
-  /* set u = [u 0]^T */
-  u[iq] = 0.0;
-  /* add ip to the active set A */
-  A[iq] = ip;
-  
-#ifdef TRACE_SOLVER
-  std::cout << "Trying with constraint " << ip << std::endl;
-  print_vector("np", np);
-#endif
-  
-l2a:/* Step 2a: determine step direction */
-    /* compute z = H np: the step direction in the primal space (through J, see the paper) */
-    compute_d(d, J, np);
-  update_z(z, J, d, iq);
-  /* compute N* np (if q > 0): the negative of the step direction in the dual space */
-  update_r(R, r, d, iq);
-#ifdef TRACE_SOLVER
-  std::cout << "Step direction z" << std::endl;
-  print_vector("z", z);
-  print_vector("r", r, iq + 1);
-  print_vector("u", u, iq + 1);
-  print_vector("d", d);
-  print_vector("A", A, iq + 1);
-#endif
-  
-  /* Step 2b: compute step length */
-  l = 0;
-  /* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
-  t1 = inf; /* +inf */
-  /* find the index l s.t. it reaches the minimum of u+[x] / r */
-  for (k = p; k < iq; k++)
-  {
-    if (r[k] > 0.0)
-    {
-      if (u[k] / r[k] < t1)
-	    {
-	      t1 = u[k] / r[k];
-	      l = A[k];
-	    }
-    }
-  }
-  /* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
-  if (fabs(scalar_product(z, z))  > std::numeric_limits<double>::epsilon()) // i.e. z != 0
-  {
-    t2 = -s[ip] / scalar_product(z, np);
-    if (t2 < 0) // patch suggested by Takano Akio for handling numerical inconsistencies
-      t2 = inf;
-  }
-  else
-    t2 = inf; /* +inf */
-  
-  /* the step is chosen as the minimum of t1 and t2 */
-  t = std::min(t1, t2);
-#ifdef TRACE_SOLVER
-  std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
-#endif
-  
-  /* Step 2c: determine new S-pair and take step: */
-  
-  /* case (i): no step in primal or dual space */
-  if (t >= inf)
-  {
-    /* QPP is infeasible */
-    return inf;
-  }
-  /* case (ii): step in dual space */
-  if (t2 >= inf)
-  {
-    /* set u = u +  t * [-r 1] and drop constraint l from the active set A */
-    for (k = 0; k < iq; k++)
-      u[k] -= t * r[k];
-    u[iq] += t;
-    iai[l] = l;
-    delete_constraint(R, J, A, u, n, p, iq, l);
-#ifdef TRACE_SOLVER
-    std::cout << " in dual space: " 
-      << f_value << std::endl;
-    print_vector("x", x);
-    print_vector("z", z);
-    print_vector("A", A, iq + 1);
-#endif
-    goto l2a;
-  }
-  
-  /* case (iii): step in primal and dual space */
-  
-  /* set x = x + t * z */
-  for (k = 0; k < n; k++)
-    x[k] += t * z[k];
-  /* update the solution value */
-  f_value += t * scalar_product(z, np) * (0.5 * t + u[iq]);
-  /* u = u + t * [-r 1] */
-  for (k = 0; k < iq; k++)
-    u[k] -= t * r[k];
-  u[iq] += t;
-#ifdef TRACE_SOLVER
-  std::cout << " in both spaces: " 
-    << f_value << std::endl;
-  print_vector("x", x);
-  print_vector("u", u, iq + 1);
-  print_vector("r", r, iq + 1);
-  print_vector("A", A, iq + 1);
-#endif
-  
-  if (fabs(t - t2) < std::numeric_limits<double>::epsilon())
-  {
-#ifdef TRACE_SOLVER
-    std::cout << "Full step has taken " << t << std::endl;
-    print_vector("x", x);
-#endif
-    /* full step has taken */
-    /* add constraint ip to the active set*/
-    if (!add_constraint(R, J, d, iq, R_norm))
-    {
-      iaexcl[ip] = false;
-      delete_constraint(R, J, A, u, n, p, iq, ip);
-#ifdef TRACE_SOLVER
-      print_matrix("R", R);
-      print_vector("A", A, iq);
-			print_vector("iai", iai);
-#endif
-      for (i = 0; i < m; i++)
-        iai[i] = i;
-      for (i = p; i < iq; i++)
-	    {
-	      A[i] = A_old[i];
-	      u[i] = u_old[i];
-				iai[A[i]] = -1;
-	    }
-      for (i = 0; i < n; i++)
-        x[i] = x_old[i];
-      goto l2; /* go to step 2 */
-    }    
-    else
-      iai[ip] = -1;
-#ifdef TRACE_SOLVER
-    print_matrix("R", R);
-    print_vector("A", A, iq);
-		print_vector("iai", iai);
-#endif
-    goto l1;
-  }
-  
-  /* a patial step has taken */
-#ifdef TRACE_SOLVER
-  std::cout << "Partial step has taken " << t << std::endl;
-  print_vector("x", x);
-#endif
-  /* drop constraint l */
-  iai[l] = l;
-  delete_constraint(R, J, A, u, n, p, iq, l);
-#ifdef TRACE_SOLVER
-  print_matrix("R", R);
-  print_vector("A", A, iq);
-#endif
-  
-  /* update s[ip] = CI * x + ci0 */
-  sum = 0.0;
-  for (k = 0; k < n; k++)
-    sum += CI[k][ip] * x[k];
-  s[ip] = sum + ci0[ip];
-  
-#ifdef TRACE_SOLVER
-  print_vector("s", s, m);
-#endif
-  goto l2a;
-}
-
-inline void compute_d(Vector<double>& d, const Matrix<double>& J, const Vector<double>& np)
-{
-  register int i, j, n = d.size();
-  register double sum;
-  
-  /* compute d = H^T * np */
-  for (i = 0; i < n; i++)
-  {
-    sum = 0.0;
-    for (j = 0; j < n; j++)
-      sum += J[j][i] * np[j];
-    d[i] = sum;
-  }
-}
-
-inline void update_z(Vector<double>& z, const Matrix<double>& J, const Vector<double>& d, int iq)
-{
-  register int i, j, n = z.size();
-	
-  /* setting of z = H * d */
-  for (i = 0; i < n; i++)
-  {
-    z[i] = 0.0;
-    for (j = iq; j < n; j++)
-      z[i] += J[i][j] * d[j];
-  }
-}
-
-inline void update_r(const Matrix<double>& R, Vector<double>& r, const Vector<double>& d, int iq)
-{
-  register int i, j;
-  register double sum;
-  
-  /* setting of r = R^-1 d */
-  for (i = iq - 1; i >= 0; i--)
-  {
-    sum = 0.0;
-    for (j = i + 1; j < iq; j++)
-      sum += R[i][j] * r[j];
-    r[i] = (d[i] - sum) / R[i][i];
-  }
-}
-
-bool add_constraint(Matrix<double>& R, Matrix<double>& J, Vector<double>& d, unsigned int& iq, double& R_norm)
-{
-  unsigned int n = d.size();
-#ifdef TRACE_SOLVER
-  std::cout << "Add constraint " << iq << '/';
-#endif
-  register unsigned int i, j, k;
-  double cc, ss, h, t1, t2, xny;
-	
-  /* we have to find the Givens rotation which will reduce the element
-    d[j] to zero.
-    if it is already zero we don't have to do anything, except of
-    decreasing j */  
-  for (j = n - 1; j >= iq + 1; j--)
-  {
-    /* The Givens rotation is done with the matrix (cc cs, cs -cc).
-    If cc is one, then element (j) of d is zero compared with element
-    (j - 1). Hence we don't have to do anything. 
-    If cc is zero, then we just have to switch column (j) and column (j - 1) 
-    of J. Since we only switch columns in J, we have to be careful how we
-    update d depending on the sign of gs.
-    Otherwise we have to apply the Givens rotation to these columns.
-    The i - 1 element of d has to be updated to h. */
-    cc = d[j - 1];
-    ss = d[j];
-    h = distance(cc, ss);
-    if (fabs(h) < std::numeric_limits<double>::epsilon()) // h == 0
-      continue;
-    d[j] = 0.0;
-    ss = ss / h;
-    cc = cc / h;
-    if (cc < 0.0)
-    {
-      cc = -cc;
-      ss = -ss;
-      d[j - 1] = -h;
-    }
-    else
-      d[j - 1] = h;
-    xny = ss / (1.0 + cc);
-    for (k = 0; k < n; k++)
-    {
-      t1 = J[k][j - 1];
-      t2 = J[k][j];
-      J[k][j - 1] = t1 * cc + t2 * ss;
-      J[k][j] = xny * (t1 + J[k][j - 1]) - t2;
-    }
-  }
-  /* update the number of constraints added*/
-  iq++;
-  /* To update R we have to put the iq components of the d vector
-    into column iq - 1 of R
-    */
-  for (i = 0; i < iq; i++)
-    R[i][iq - 1] = d[i];
-#ifdef TRACE_SOLVER
-  std::cout << iq << std::endl;
-  print_matrix("R", R, iq, iq);
-  print_matrix("J", J);
-  print_vector("d", d, iq);
-#endif
-  
-  if (fabs(d[iq - 1]) <= std::numeric_limits<double>::epsilon() * R_norm) 
-  {
-    // problem degenerate
-    return false;
-  }
-  R_norm = std::max<double>(R_norm, fabs(d[iq - 1]));
-  return true;
-}
-
-void delete_constraint(Matrix<double>& R, Matrix<double>& J, Vector<int>& A, Vector<double>& u, unsigned int n, int p, unsigned int& iq, int l)
-{
-#ifdef TRACE_SOLVER
-  std::cout << "Delete constraint " << l << ' ' << iq;
-#endif
-  register unsigned int i, j, k, qq = 0; // just to prevent warnings from smart compilers
-  double cc, ss, h, xny, t1, t2;
-
-  bool found = false;
-  /* Find the index qq for active constraint l to be removed */
-  for (i = p; i < iq; i++)
-    if (A[i] == l)
-    {
-      qq = i;
-      found = true;
-      break;
-    }
-
-  if(!found)
-  {
-    std::ostringstream os;
-    os << "Attempt to delete non existing constraint, constraint: " << l;
-    throw std::invalid_argument(os.str());
-  }
-  /* remove the constraint from the active set and the duals */
-  for (i = qq; i < iq - 1; i++)
-    {
-      A[i] = A[i + 1];
-      u[i] = u[i + 1];
-      for (j = 0; j < n; j++)
-        R[j][i] = R[j][i + 1];
-    }
-      
-  A[iq - 1] = A[iq];
-  u[iq - 1] = u[iq];
-  A[iq] = 0; 
-  u[iq] = 0.0;
-  for (j = 0; j < iq; j++)
-    R[j][iq - 1] = 0.0;
-  /* constraint has been fully removed */
-  iq--;
-#ifdef TRACE_SOLVER
-  std::cout << '/' << iq << std::endl;
-#endif 
-  
-  if (iq == 0)
-    return;
-  
-  for (j = qq; j < iq; j++)
-  {
-    cc = R[j][j];
-    ss = R[j + 1][j];
-    h = distance(cc, ss);
-    if (fabs(h) < std::numeric_limits<double>::epsilon()) // h == 0
-      continue;
-    cc = cc / h;
-    ss = ss / h;
-    R[j + 1][j] = 0.0;
-    if (cc < 0.0)
-    {
-      R[j][j] = -h;
-      cc = -cc;
-      ss = -ss;
-    }
-    else
-      R[j][j] = h;
-    
-    xny = ss / (1.0 + cc);
-    for (k = j + 1; k < iq; k++)
-    {
-      t1 = R[j][k];
-      t2 = R[j + 1][k];
-      R[j][k] = t1 * cc + t2 * ss;
-      R[j + 1][k] = xny * (t1 + R[j][k]) - t2;
-    }
-    for (k = 0; k < n; k++)
-    {
-      t1 = J[k][j];
-      t2 = J[k][j + 1];
-      J[k][j] = t1 * cc + t2 * ss;
-      J[k][j + 1] = xny * (J[k][j] + t1) - t2;
-    }
-  }
-}
-
-inline double distance(double a, double b)
-{
-  register double a1, b1, t;
-  a1 = fabs(a);
-  b1 = fabs(b);
-  if (a1 > b1) 
-  {
-    t = (b1 / a1);
-    return a1 * sqrt(1.0 + t * t);
-  }
-  else
-    if (b1 > a1)
-    {
-      t = (a1 / b1);
-      return b1 * sqrt(1.0 + t * t);
-    }
-  return a1 * sqrt(2.0);
-}
-
-
-inline double scalar_product(const Vector<double>& x, const Vector<double>& y)
-{
-  register int i, n = x.size();
-  register double sum;
-	
-  sum = 0.0;
-  for (i = 0; i < n; i++)
-    sum += x[i] * y[i];
-  return sum;			
-}
-
-void cholesky_decomposition(Matrix<double>& A) 
-{
-  register int i, j, k, n = A.nrows();
-  register double sum;
-	
-  for (i = 0; i < n; i++)
-  {
-    for (j = i; j < n; j++)
-    {
-      sum = A[i][j];
-      for (k = i - 1; k >= 0; k--)
-        sum -= A[i][k]*A[j][k];
-      if (i == j) 
-	    {
-	      if (sum <= 0.0)
-        {
-          std::ostringstream os;
-          // raise error
-          print_matrix("A", A);
-          os << "Error in cholesky decomposition, sum: " << sum;
-          throw std::logic_error(os.str());
-          exit(-1);
-        }
-	      A[i][i] = sqrt(sum);
-	    }
-      else
-        A[j][i] = sum / A[i][i];
-    }
-    for (k = i + 1; k < n; k++)
-      A[i][k] = A[k][i];
-  } 
-}
-
-void cholesky_solve(const Matrix<double>& L, Vector<double>& x, const Vector<double>& b)
-{
-  int n = L.nrows();
-  Vector<double> y(n);
-	
-  /* Solve L * y = b */
-  forward_elimination(L, y, b);
-  /* Solve L^T * x = y */
-  backward_elimination(L, x, y);
-}
-
-inline void forward_elimination(const Matrix<double>& L, Vector<double>& y, const Vector<double>& b)
-{
-  register int i, j, n = L.nrows();
-	
-  y[0] = b[0] / L[0][0];
-  for (i = 1; i < n; i++)
-  {
-    y[i] = b[i];
-    for (j = 0; j < i; j++)
-      y[i] -= L[i][j] * y[j];
-    y[i] = y[i] / L[i][i];
-  }
-}
-
-inline void backward_elimination(const Matrix<double>& U, Vector<double>& x, const Vector<double>& y)
-{
-  register int i, j, n = U.nrows();
-	
-  x[n - 1] = y[n - 1] / U[n - 1][n - 1];
-  for (i = n - 2; i >= 0; i--)
-  {
-    x[i] = y[i];
-    for (j = i + 1; j < n; j++)
-      x[i] -= U[i][j] * x[j];
-    x[i] = x[i] / U[i][i];
-  }
-}
-
-void print_matrix(const char* name, const Matrix<double>& A, int n, int m)
-{
-  std::ostringstream s;
-  std::string t;
-  if (n == -1)
-    n = A.nrows();
-  if (m == -1)
-    m = A.ncols();
-	
-  s << name << ": " << std::endl;
-  for (int i = 0; i < n; i++)
-  {
-    s << " ";
-    for (int j = 0; j < m; j++)
-      s << A[i][j] << ", ";
-    s << std::endl;
-  }
-  t = s.str();
-  t = t.substr(0, t.size() - 3); // To remove the trailing space, comma and newline
-	
-  std::cout << t << std::endl;
-}
-
-template<typename T>
-void print_vector(const char* name, const Vector<T>& v, int n)
-{
-  std::ostringstream s;
-  std::string t;
-  if (n == -1)
-    n = v.size();
-	
-  s << name << ": " << std::endl << " ";
-  for (int i = 0; i < n; i++)
-  {
-    s << v[i] << ", ";
-  }
-  t = s.str();
-  t = t.substr(0, t.size() - 2); // To remove the trailing space and comma
-	
-  std::cout << t << std::endl;
-}
-
-}  // namespace quadprogpp

thirdparty/tinyxml/src/tinystr.cpp

@@ -1,111 +0,0 @@
-/*
-www.sourceforge.net/projects/tinyxml
-
-This software is provided 'as-is', without any express or implied
-warranty. In no event will the authors be held liable for any
-damages arising from the use of this software.
-
-Permission is granted to anyone to use this software for any
-purpose, including commercial applications, and to alter it and
-redistribute it freely, subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must
-not claim that you wrote the original software. If you use this
-software in a product, an acknowledgment in the product documentation
-would be appreciated but is not required.
-
-2. Altered source versions must be plainly marked as such, and
-must not be misrepresented as being the original software.
-
-3. This notice may not be removed or altered from any source
-distribution.
-*/
-
-
-#ifndef TIXML_USE_STL
-
-#include "tinyxml/include/tinystr.h"
-
-// Error value for find primitive
-const TiXmlString::size_type TiXmlString::npos = static_cast< TiXmlString::size_type >(-1);
-
-
-// Null rep.
-TiXmlString::Rep TiXmlString::nullrep_ = { 0, 0, { '\0' } };
-
-
-void TiXmlString::reserve (size_type cap)
-{
-	if (cap > capacity())
-	{
-		TiXmlString tmp;
-		tmp.init(length(), cap);
-		memcpy(tmp.start(), data(), length());
-		swap(tmp);
-	}
-}
-
-
-TiXmlString& TiXmlString::assign(const char* str, size_type len)
-{
-	size_type cap = capacity();
-	if (len > cap || cap > 3*(len + 8))
-	{
-		TiXmlString tmp;
-		tmp.init(len);
-		memcpy(tmp.start(), str, len);
-		swap(tmp);
-	}
-	else
-	{
-		memmove(start(), str, len);
-		set_size(len);
-	}
-	return *this;
-}
-
-
-TiXmlString& TiXmlString::append(const char* str, size_type len)
-{
-	size_type newsize = length() + len;
-	if (newsize > capacity())
-	{
-		reserve (newsize + capacity());
-	}
-	memmove(finish(), str, len);
-	set_size(newsize);
-	return *this;
-}
-
-
-TiXmlString operator + (const TiXmlString & a, const TiXmlString & b)
-{
-	TiXmlString tmp;
-	tmp.reserve(a.length() + b.length());
-	tmp += a;
-	tmp += b;
-	return tmp;
-}
-
-TiXmlString operator + (const TiXmlString & a, const char* b)
-{
-	TiXmlString tmp;
-	TiXmlString::size_type b_len = static_cast<TiXmlString::size_type>( strlen(b) );
-	tmp.reserve(a.length() + b_len);
-	tmp += a;
-	tmp.append(b, b_len);
-	return tmp;
-}
-
-TiXmlString operator + (const char* a, const TiXmlString & b)
-{
-	TiXmlString tmp;
-	TiXmlString::size_type a_len = static_cast<TiXmlString::size_type>( strlen(a) );
-	tmp.reserve(a_len + b.length());
-	tmp.append(a, a_len);
-	tmp += b;
-	return tmp;
-}
-
-
-#endif	// TIXML_USE_STL

thirdparty/tinyxml/src/tinyxmlerror.cpp

@@ -1,52 +0,0 @@
-/*
-www.sourceforge.net/projects/tinyxml
-Original code (2.0 and earlier )copyright (c) 2000-2006 Lee Thomason (www.grinninglizard.com)
-
-This software is provided 'as-is', without any express or implied 
-warranty. In no event will the authors be held liable for any 
-damages arising from the use of this software.
-
-Permission is granted to anyone to use this software for any 
-purpose, including commercial applications, and to alter it and 
-redistribute it freely, subject to the following restrictions:
-
-1. The origin of this software must not be misrepresented; you must
-not claim that you wrote the original software. If you use this
-software in a product, an acknowledgment in the product documentation
-would be appreciated but is not required.
-
-2. Altered source versions must be plainly marked as such, and
-must not be misrepresented as being the original software.
-
-3. This notice may not be removed or altered from any source
-distribution.
-*/
-
-#include "tinyxml/include/tinyxml.h"
-
-// The goal of the seperate error file is to make the first
-// step towards localization. tinyxml (currently) only supports
-// english error messages, but the could now be translated.
-//
-// It also cleans up the code a bit.
-//
-
-const char* TiXmlBase::errorString[ TiXmlBase::TIXML_ERROR_STRING_COUNT ] =
-{
-	"No error",
-	"Error",
-	"Failed to open file",
-	"Error parsing Element.",
-	"Failed to read Element name",
-	"Error reading Element value.",
-	"Error reading Attributes.",
-	"Error: empty tag.",
-	"Error reading end tag.",
-	"Error parsing Unknown.",
-	"Error parsing Comment.",
-	"Error parsing Declaration.",
-	"Error document empty.",
-	"Error null (0) or unexpected EOF found in input stream.",
-	"Error parsing CDATA.",
-	"Error when TiXmlDocument added to document, because TiXmlDocument can only be at the root.",
-};