diff --git a/CMakeLists.txt b/CMakeLists.txt index 192c216..01ba915 100644 --- a/CMakeLists.txt +++ b/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 ${catkin_LIBRARIES} libUnitree_motor_SDK_Linux64_EPOLL.so libZ1_${COMMUNICATION}_Linux64.so) - -# set(EXECUTABLE_OUTPUT_PATH /home/$ENV{USER}/) +target_link_libraries(z1_ctrl libZ1_${COMMUNICATION}_Linux64.so ${catkin_LIBRARIES} ) diff --git a/config.xml b/config.xml index 6547186..b3b6a9a 100644 --- a/config.xml +++ b/config.xml @@ -1,26 +1,17 @@ - + keyboard sdk joystick - - - - - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - + 192.168.123.110 + 8881 + + + N + 10.0 + 0.0 + diff --git a/config/savedArmStates.csv b/config/savedArmStates.csv index 0201ef6..c2a67bb 100755 --- a/config/savedArmStates.csv +++ b/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, -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.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, -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, -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, -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, -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, -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, -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, -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, -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, -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, -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, -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, +forward, 0.0, 1.5, -1.0, -0.54, 0.0, 0.0, +startFlat, 0.0, 0.0, -0.005, -0.074, 0.0, 0.0, +search, 0.0, 0.0, -0.005, -0.5, 0.0, 0.0, +show_left, -1.0, 0.9, -1., -0.3, 0.0, 0.0, +show_mid, 0.0, 0.9, -1.2, 0.3, 0.0, 0.0, +show_right, 1.0, 0.9, -1., -0.3, 0.0, 0.0, +5kg1, 0.661051, 3.010641, -2.885593, -0.233059, 0.0, 0.0, +5kg2, -0.495523, 3.122645, -2.856166, -0.406859, 0.0, 0.0, +mh1, 2.617994, 0.084000, -2.755093, 1.470296, 1.454343, 2.645527, +mh2, -2.485994, 2.451000, -0.102000, -1.611829, -1.421343, -2.265473, +01, 0.028072, 0.343213, -0.010000, -0.073124, 0.084670, -0.016248, +02, 2.024087, 0.760020, -0.816744, -1.190535, 1.175596, 2.064632, +03, -1.740466, 2.731333, -2.161190, 0.806278, -0.889325, -2.529499, \ No newline at end of file diff --git a/include/FSM/BaseState.h b/include/FSM/BaseState.h index b070c9b..43d9071 100755 --- a/include/FSM/BaseState.h +++ b/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 \ No newline at end of file diff --git a/include/FSM/FSMState.h b/include/FSM/FSMState.h index 920f531..a983cc8 100755 --- a/include/FSM/FSMState.h +++ b/include/FSM/FSMState.h @@ -5,15 +5,9 @@ #include #include #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 "unitree_arm_sdk/timer.h" -#include "model/ArmDynKineModel.h" - -#include "FSM/FiniteStateMachine.h" +#include "common/utilities/timer.h" +#include "FSM/BaseState.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 _tauFriction(); - 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; + void _recordData(); + Vec6 _postureToVec6(Posture posture); - double _gripperLinearFriction; - double _gripperCoulombFriction; - static double _gripperCoulombDirection; - double _gripperFriction; - - CtrlComponents *_ctrlComp; - ArmFSMStateName _nextStateName; - - IOInterface *_ioInter; LowlevelCmd *_lowCmd; LowlevelState *_lowState; - UserValue _userValue; - ArmDynKineModel *_armModel; + IOInterface *_ioInter; + ArmModel *_armModel; - Vec6 _qPast, _qdPast, _q, _qd, _qdd; - Vec6 _tau, _endForce; + Vec6 _qPast, _qdPast, _q, _qd, _qdd, _tauf, _tauCmd, _g; + double _gripperPos, _gripperW, _gripperTau; + uint _collisionCnt; - Vec6 _coulombFriction; - static Vec6 _coulombDirection; - Vec6 _linearFriction; - - Vec6 _kpDiag, _kdDiag; - Mat6 _Kp, _Kd; - - std::vector _jointQMax; - std::vector _jointQMin; - std::vector _jointSpeedMax; - - SendCmd _sendCmd; - RecvState _recvState; + CtrlComponents *_ctrlComp; }; #endif // FSMSTATE_H diff --git a/include/FSM/FiniteStateMachine.h b/include/FSM/FiniteStateMachine.h index 71fdbe8..e6d1df7 100755 --- a/include/FSM/FiniteStateMachine.h +++ b/include/FSM/FiniteStateMachine.h @@ -2,40 +2,26 @@ #define FSM_H #include -#include "FSM/BaseState.h" -#include "unitree_arm_sdk/cmdPanel.h" -#include "unitree_arm_sdk/loop.h" -#include "unitree_arm_sdk/keyboard.h" +#include "FSM/FSMState.h" +#include "common/utilities/loop.h" +#include "control/CtrlComponents.h" -enum class FSMRunMode{ - NORMAL, - CHANGE -}; - -/* - 状态机初始状态为states的第一个元素 - 为了支持多个状态机从同一个CmdPanel获取触发信号， - 加入了cmdChannel，不同状态机之间的cmdChannel必须不同， - 否则可能会漏过状态触发 -*/ class FiniteStateMachine{ public: - FiniteStateMachine(std::vector states, - CmdPanel *cmdPanel, size_t cmdChannel = 0, double dt=0.002); + FiniteStateMachine(std::vector states, CtrlComponents *ctrlComp); virtual ~FiniteStateMachine(); private: void _run(); - std::vector _states; + std::vector _states; - FSMRunMode _mode; + FSMMode _mode; bool _running; - BaseState* _currentState; - BaseState* _nextState; + FSMState* _currentState; + FSMState* _nextState; int _nextStateEnum; - size_t _cmdChannel; - CmdPanel *_cmdPanel; + CtrlComponents *_ctrlComp; LoopFunc *_runThread; }; diff --git a/include/FSM/State_BackToStart.h b/include/FSM/State_BackToStart.h index a77434a..9399e13 100644 --- a/include/FSM/State_BackToStart.h +++ b/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(); - void _setTrajSDK(){} + bool _reach, _pastReach; + JointSpaceTraj *_jointTraj; }; #endif // STATE_BACKTOSTART_H \ No newline at end of file diff --git a/include/FSM/State_Cartesian.h b/include/FSM/State_Cartesian.h index 43ac293..bc88ec8 100755 --- a/include/FSM/State_Cartesian.h +++ b/include/FSM/State_Cartesian.h @@ -12,17 +12,15 @@ public: void exit(); int checkChange(int cmd); private: - double _posSpeed; - double _oriSpeed; - Vec3 _omega; - Vec3 _velocity; + double _oriSpeed = 0.4;// control by keyboard or joystick + double _posSpeed = 0.15; + double oriSpeedLimit = 0.3;// limits in SDK + 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 \ No newline at end of file diff --git a/include/FSM/State_Dance.h b/include/FSM/State_Dance.h deleted file mode 100644 index 1ef5039..0000000 --- a/include/FSM/State_Dance.h +++ /dev/null @@ -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 \ No newline at end of file diff --git a/include/FSM/State_JointSpace.h b/include/FSM/State_JointSpace.h index 75a4b29..4b7bbdd 100755 --- a/include/FSM/State_JointSpace.h +++ b/include/FSM/State_JointSpace.h @@ -12,7 +12,7 @@ public: void exit(); int checkChange(int cmd); private: - double _angleStep; + std::vector jointSpeedMax; }; #endif // JOINTSPACE_H \ No newline at end of file diff --git a/include/FSM/State_MoveC.h b/include/FSM/State_MoveC.h index 9b77aab..c27a5f7 100755 --- a/include/FSM/State_MoveC.h +++ b/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 _posSpeed; - double _oriSpeed; - Vec3 _omega; - 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; + double _speed; + std::vector _postures; + EndCircleTraj *_circleTraj; + bool _timeReached, _taskReached, _pastTaskReached, _finalReached; }; #endif // CARTESIAN_H \ No newline at end of file diff --git a/include/FSM/State_MoveJ.h b/include/FSM/State_MoveJ.h index def1e20..48cffac 100755 --- a/include/FSM/State_MoveJ.h +++ b/include/FSM/State_MoveJ.h @@ -13,11 +13,10 @@ public: void exit(); int checkChange(int cmd); private: - double _costTime; - HomoMat _goalHomo; + double _speed; JointSpaceTraj *_jointTraj; bool _reached, _pastReached, _finalReached; - std::vector _result; + std::vector _qCmd; }; #endif // CARTESIAN_H \ No newline at end of file diff --git a/include/FSM/State_MoveL.h b/include/FSM/State_MoveL.h index 6bde0e4..fc16132 100755 --- a/include/FSM/State_MoveL.h +++ b/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 _posSpeed; - double _oriSpeed; - Vec3 _omega; - Vec3 _velocity; - Vec6 _twist; - Vec6 _pastPosture, _endPosture, _endTwist; - HomoMat _endHomoFeedback; - Vec6 _Pdes; - Vec6 _Pfd; - Vec6 _Pkp; - Vec6 _endPostureError; - - // 轨迹相关变量 - std::vector > _posture; - CartesionSpaceTraj *_cartesionTraj; + double _speed; + std::vector _postures; + EndLineTraj *_lineTraj; bool _timeReached, _taskReached, _pastTaskReached, _finalReached; }; #endif // CARTESIAN_H \ No newline at end of file diff --git a/include/FSM/State_Passive.h b/include/FSM/State_Passive.h index 506e215..b4c4306 100755 --- a/include/FSM/State_Passive.h +++ b/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: diff --git a/include/FSM/State_SaveState.h b/include/FSM/State_SaveState.h index cdae1a0..5393fc7 100755 --- a/include/FSM/State_SaveState.h +++ b/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 \ No newline at end of file diff --git a/include/FSM/State_Teach.h b/include/FSM/State_Teach.h index 533a3cd..0bfcbb9 100755 --- a/include/FSM/State_Teach.h +++ b/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: diff --git a/include/FSM/State_TeachRepeat.h b/include/FSM/State_TeachRepeat.h index aad6bbf..e79802e 100755 --- a/include/FSM/State_TeachRepeat.h +++ b/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: diff --git a/include/FSM/State_ToState.h b/include/FSM/State_ToState.h index 7b1a82e..066ceb3 100755 --- a/include/FSM/State_ToState.h +++ b/include/FSM/State_ToState.h @@ -19,7 +19,6 @@ private: JointSpaceTraj *_jointTraj; bool _reach, _pastReach; std::string _goalName; - // CSVTool *_csv; }; #endif // TOSTATE_H \ No newline at end of file diff --git a/include/FSM/State_Trajectory.h b/include/FSM/State_Trajectory.h index 8bad4c7..ea5c47d 100755 --- a/include/FSM/State_Trajectory.h +++ b/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; - virtual void _setTraj(); - virtual void _setTrajSdk(); + void _setTraj(); + void _setTrajSDK(); TrajectoryManager *_traj; HomoMat _goalHomo; Vec6 _goalTwist; + double speedTemp; JointSpaceTraj *_toStartTraj; bool _reachedStart = false; @@ -31,11 +30,8 @@ protected: std::vector _stopTraj; std::vector _lineTraj; - long long startTime; static std::vector _trajCmd; Vec6 _posture[2]; - - static bool _isTrajFSM; }; #endif // CARTESIANPATH_H \ No newline at end of file diff --git a/include/common/enumClass.h b/include/common/enumClass.h index 9786c46..b6542d0 100755 --- a/include/common/enumClass.h +++ b/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, - _SDK, - _JOYSTICK, + KEYBOARD, + SDK, + JOYSTICK }; #endif // ENUMCLASS_H \ No newline at end of file diff --git a/include/common/utilities/LPFilter.h b/include/common/math/Filter.h old mode 100755 new mode 100644 similarity index 88% rename from include/common/utilities/LPFilter.h rename to include/common/math/Filter.h index f9dc43c..ec421da --- a/include/common/utilities/LPFilter.h +++ b/include/common/math/Filter.h @@ -1,5 +1,5 @@ -#ifndef LOWPASSFILTER -#define LOWPASSFILTER +#ifndef FILTER +#define FILTER #include #include @@ -26,4 +26,4 @@ private: bool _start; }; -#endif // LOWPASSFILTER \ No newline at end of file +#endif // FILTER \ No newline at end of file diff --git a/include/common/math/mathTools.h b/include/common/math/mathTools.h index 59d35a8..11f812a 100755 --- a/include/common/math/mathTools.h +++ b/include/common/math/mathTools.h @@ -2,18 +2,9 @@ #define MATHTOOLS_H #include +#include #include "common/math/mathTypes.h" -// template -// inline T1 max(const T1 a, const T2 b){ -// return (a > b ? a : b); -// } - -// template -// inline T1 min(const T1 a, const T2 b){ -// return (a < b ? a : b); -// } - template T max(T value){ return value; @@ -34,21 +25,6 @@ inline T min(const T val0, const Args... restVal){ return val0 > (min(restVal...)) ? val0 : min(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 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: diff --git a/include/common/math/mathTypes.h b/include/common/math/mathTypes.h index 2ca09b1..b74c7d7 100755 --- a/include/common/math/mathTypes.h +++ b/include/common/math/mathTypes.h @@ -2,6 +2,7 @@ #define MATHTYPES_H #include +#include /************************/ /******** Vector ********/ @@ -96,4 +97,13 @@ inline VecX stdVecToEigenVec(T stdVec){ VecX eigenVec = Eigen::VectorXd::Map(&stdVec[0], stdVec.size()); return eigenVec; } + +inline std::vector EigenVectostdVec(VecX eigenVec){ + std::vector stdVec; + for(int i(0); i namespace quadprogpp { diff --git a/include/common/math/robotics.h b/include/common/math/robotics.h index 98cbd0d..3b30766 100644 --- a/include/common/math/robotics.h +++ b/include/common/math/robotics.h @@ -3,8 +3,7 @@ #include #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&, 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&, const std::vector&, - 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 ForwardDynamicsTrajectory(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::MatrixXd&, - const Eigen::VectorXd&, const Eigen::MatrixXd&, const std::vector&, const std::vector&, - 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&, const std::vector&, 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 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 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 SimulateControl(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, - const Eigen::MatrixXd&, const std::vector&, const std::vector&, - const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, - const Eigen::VectorXd&, const std::vector&, const std::vector&, - double, double, double, double, int); - -} +} \ No newline at end of file diff --git a/include/common/utilities/CSVTool.h b/include/common/utilities/CSVTool.h index bc27544..456a182 100755 --- a/include/common/utilities/CSVTool.h +++ b/include/common/utilities/CSVTool.h @@ -181,7 +181,7 @@ inline void CSVTool::readFile(){ inline bool CSVTool::getLine(std::string label, std::vector &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); diff --git a/include/common/utilities/loop.h b/include/common/utilities/loop.h new file mode 100644 index 0000000..8369c7a --- /dev/null +++ b/include/common/utilities/loop.h @@ -0,0 +1,55 @@ +#ifndef _UNITREE_ARM_LOOP_H_ +#define _UNITREE_ARM_LOOP_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "common/utilities/timer.h" + + +// constexpr int THREAD_PRIORITY = 99; // real-time priority + +typedef boost::function 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 _fp; +}; + +#endif // _UNITREE_ARM_LOOP_H_ \ No newline at end of file diff --git a/include/common/utilities/timer.h b/include/common/utilities/timer.h new file mode 100644 index 0000000..40701da --- /dev/null +++ b/include/common/utilities/timer.h @@ -0,0 +1,56 @@ +#ifndef _UNITREE_ARM_TIMER_H_ +#define _UNITREE_ARM_TIMER_H_ + +#include +#include +#include +#include +#include + +//时间戳 微秒级， 需要#include +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 \ No newline at end of file diff --git a/thirdparty/tinyxml/include/tinystr.h b/include/common/utilities/tinyxml/tinystr.h similarity index 100% rename from thirdparty/tinyxml/include/tinystr.h rename to include/common/utilities/tinyxml/tinystr.h diff --git a/thirdparty/tinyxml/include/tinyxml.h b/include/common/utilities/tinyxml/tinyxml.h similarity index 100% rename from thirdparty/tinyxml/include/tinyxml.h rename to include/common/utilities/tinyxml/tinyxml.h diff --git a/include/control/CtrlComponents.h b/include/control/CtrlComponents.h index 68c2c0b..f3a4ab4 100755 --- a/include/control/CtrlComponents.h +++ b/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 #include -#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; - LowlevelState *lowState; + CmdPanel *cmdPanel; IOInterface *ioInter; - ArmDynKineModel *armModel; + ArmModel *armModel; CSVTool *stateCSV; + + SendCmd sendCmd; // cmd that receive from SDK + RecvState recvState;// state that send to SDK //config - Control ctrl; - bool _hasGripper; - std::string ctrl_IP; - 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; - double _endEffectorMass; - Vec3 _endEffectorCom; - Mat3 _endEffectorInertia; + std::string ctrl_IP; + uint ctrl_port; + double _loadWeight; }; #endif // CTRLCOMPONENTS_H \ No newline at end of file diff --git a/include/control/armSDK.h b/include/control/armSDK.h new file mode 100644 index 0000000..b60c215 --- /dev/null +++ b/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 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 \ No newline at end of file diff --git a/include/control/cmdPanel.h b/include/control/cmdPanel.h new file mode 100644 index 0000000..66b9e96 --- /dev/null +++ b/include/control/cmdPanel.h @@ -0,0 +1,144 @@ +#ifndef _CMDPANEL_H_ +#define _CMDPANEL_H_ + +#include +#include +#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 events, + EmptyAction emptyAction, size_t channelNum = 1, double dt = 0.002); + virtual ~CmdPanel(); + virtual int getState(size_t channelID = 0); + std::vector getValues() {return _values;}; + std::vector getDValues() {return _dValues;}; + std::vector getDirections() {return _changeDirections;}; + void setValue(std::vector values); + void setValue(double value, size_t id); + virtual std::string getString(std::string slogan); + virtual std::vector stringToArray(std::string slogan); + virtual std::vector > 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 _stateEvents; + std::vector _valueEvents; + + EmptyAction _emptyAction; + size_t _actionNum = 0; + size_t _stateNum = 0; + size_t _valueNum = 0; + size_t _channelNum; + std::vector _values; + std::vector _dValues; + std::vector _changeDirections; + int _state; + std::vector> _stateQueue; + std::vector _outputState; + std::vector _getState; + double _dt; + KeyCmd _keyCmd; + std::string _cPast = ""; + + bool _running = true; +}; + +#endif \ No newline at end of file diff --git a/include/control/joystick.h b/include/control/joystick.h new file mode 100644 index 0000000..e8b7ff1 --- /dev/null +++ b/include/control/joystick.h @@ -0,0 +1,107 @@ +#ifndef _UNITREE_ARM_JOYSTICK_H_ +#define _UNITREE_ARM_JOYSTICK_H_ + +#include +#include "message/udp.h" +#include "control/cmdPanel.h" +#include "message/joystick_common.h" +#include "message/aliengo_common.h" +#include "message/b1_common.h" +#include + +using namespace UNITREE_LEGGED_SDK_ALIENGO; +// using namespace UNITREE_LEGGED_SDK_B1; + +class UnitreeJoystick : public CmdPanel{ +public: + UnitreeJoystick(std::vector 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_ diff --git a/include/control/keyboard.h b/include/control/keyboard.h new file mode 100644 index 0000000..c0d0f08 --- /dev/null +++ b/include/control/keyboard.h @@ -0,0 +1,34 @@ +#ifndef _UNITREE_ARM_KEYBOARD_H_ +#define _UNITREE_ARM_KEYBOARD_H_ + +#include +#include +#include + +#include "message/udp.h" +#include "common/enumClass.h" +#include "control/cmdPanel.h" + +class Keyboard : public CmdPanel{ +public: + Keyboard(std::vector events, + EmptyAction emptyAction, size_t channelNum = 1, double dt = 0.002); + ~Keyboard(); + std::string getString(std::string slogan); + std::vector stringToArray(std::string slogan); + std::vector > 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_ \ No newline at end of file diff --git a/include/interface/IOInterface.h b/include/interface/IOInterface.h index 3829e82..50571dc 100755 --- a/include/interface/IOInterface.h +++ b/include/interface/IOInterface.h @@ -1,34 +1,29 @@ #ifndef IOINTERFACE_H #define IOINTERFACE_H +#include #include "message/LowlevelCmd.h" #include "message/LowlevelState.h" -#include "unitree_arm_sdk/cmdPanel.h" -#include "unitree_arm_sdk/keyboard.h" -#include +#include "control/keyboard.h" +#include "control/joystick.h" #include "common/math/robotics.h" class IOInterface{ public: - IOInterface(CmdPanel *cmdPanel):_cmdPanel(cmdPanel){} - - virtual ~IOInterface(){delete _cmdPanel;}; + IOInterface(){} + ~IOInterface(){ + 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 stringToArray(std::string slogan){return _cmdPanel->stringToArray(slogan);} - std::vector > stringToMatrix(std::string slogan){return _cmdPanel->stringToMatrix(slogan);} virtual bool calibration(){return false;}; - - SendCmd getSendCmd(){return _cmdPanel->getSendCmd();}; - void getRecvState(RecvState recvState){ _cmdPanel->getRecvState(recvState);}; - - bool isDisConnect = false;// udp disconnection - bool _isDisConnect[7];// joint(motor) disconnection + bool checkGripper(){return hasGripper;}; + LowlevelCmd *lowCmd; + LowlevelState *lowState; + virtual bool isDisconnect(){ return false;}; + bool hasErrorState; protected: - uint16_t _isDisConnectCnt[7]; - CmdPanel *_cmdPanel; + bool hasGripper; }; #endif //IOINTERFACE_H \ No newline at end of file diff --git a/include/interface/IOROS.h b/include/interface/IOROS.h index 4683553..7eb13c0 100755 --- a/include/interface/IOROS.h +++ b/include/interface/IOROS.h @@ -5,29 +5,20 @@ #include #include "interface/IOInterface.h" -#include "unitree_arm_sdk/keyboard.h" -#include "message/unitree_legged_msgs/MotorCmd.h" -#include "message/unitree_legged_msgs/MotorState.h" +#include "message/MotorCmd.h" +#include "message/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(); diff --git a/include/interface/IOUDP.h b/include/interface/IOUDP.h index 2697889..992faa5 100644 --- a/include/interface/IOUDP.h +++ b/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]; }; diff --git a/include/message/LowlevelCmd.h b/include/message/LowlevelCmd.h index ed61262..ac8ae44 100755 --- a/include/message/LowlevelCmd.h +++ b/include/message/LowlevelCmd.h @@ -7,8 +7,6 @@ #include struct LowlevelCmd{ -private: - size_t _dof = 6; public: std::vector q; std::vector dq; @@ -16,9 +14,10 @@ public: std::vector kp; std::vector kd; - std::vector> q_cmd_data; - std::vector> dq_cmd_data; - std::vector> tau_cmd_data; + std::vector> q_data; + std::vector> dq_data; + std::vector> tauf_data; + std::vector> tau_data; LowlevelCmd(); ~LowlevelCmd(){} @@ -27,13 +26,14 @@ public: void setZeroTau(); void setZeroKp(); void setZeroKd(); - void setControlGain(); - void setControlGain(std::vector KP, std::vector KW); - void setQ(std::vector qInput); void setQ(VecX qInput); void setQd(VecX qDInput); void setTau(VecX tauInput); + + void setControlGain(); + void setControlGain(std::vector KP, std::vector 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; }; diff --git a/include/message/LowlevelState.h b/include/message/LowlevelState.h index 2117cc8..9dee0ff 100755 --- a/include/message/LowlevelState.h +++ b/include/message/LowlevelState.h @@ -3,55 +3,58 @@ #include #include -#include "common/math/mathTypes.h" #include "common/math/mathTools.h" #include "common/enumClass.h" -#include "common/utilities/LPFilter.h" -#include "message/UserValue.h" +#include "common/math/Filter.h" struct LowlevelState{ -private: - size_t _dof = 6; public: LowlevelState(double dt); ~LowlevelState(); - // MotorState motorState[12]; std::vector q; std::vector dq; std::vector ddq; std::vector tau; - std::vector temperature; + std::vector> q_data; + std::vector> dq_data; + std::vector> ddq_data; + std::vector> tau_data; + + std::vector temperature; std::vector errorstate; + std::vector isMotorConnected; std::vector qFiltered; std::vector dqFiltered; + std::vector ddqFiltered; std::vector tauFiltered; - std::vector> q_state_data; - std::vector> dq_state_data; - std::vector> 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 getGripperQd() {return dq.at(q.size()-1);} - double getGripperTau() {return tau.at(tau.size()-1);} - double getGripperTauFiltered() {return tauFiltered.at(tau.size()-1);} + double getGripperQ(); + double getGripperQd(); + double getGripperTau(); + double getGripperTauFiltered(); +private: + size_t _dof = 6; + int temporatureLimit = 80.0;// centigrade + std::vector _isMotorConnectedCnt; }; #endif //LOWLEVELSTATE_HPP diff --git a/include/message/unitree_legged_msgs/MotorCmd.h b/include/message/MotorCmd.h similarity index 100% rename from include/message/unitree_legged_msgs/MotorCmd.h rename to include/message/MotorCmd.h diff --git a/include/message/unitree_legged_msgs/MotorState.h b/include/message/MotorState.h similarity index 100% rename from include/message/unitree_legged_msgs/MotorState.h rename to include/message/MotorState.h diff --git a/include/message/UserValue.h b/include/message/UserValue.h deleted file mode 100644 index e69956a..0000000 --- a/include/message/UserValue.h +++ /dev/null @@ -1,38 +0,0 @@ -#ifndef USERVALUE_H -#define USERVALUE_H - -#include "common/math/mathTypes.h" -#include "common/utilities/typeTrans.h" -#include - -template -std::vector cutVector(std::vector vec, size_t startID, size_t length){ - std::vector result; - result.assign(vec.begin()+startID, vec.begin()+startID+length); - return result; -} - -struct UserValue{ - Vec6 moveAxis; - double gripperPos; - - void setData(std::vector 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 \ No newline at end of file diff --git a/include/message/aliengo_common.h b/include/message/aliengo_common.h new file mode 100644 index 0000000..b82e925 --- /dev/null +++ b/include/message/aliengo_common.h @@ -0,0 +1,168 @@ +#ifndef _UNITREE_ARM_ALIENGO_COMMON_H_ +#define _UNITREE_ARM_ALIENGO_COMMON_H_ + +#include + +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 \ No newline at end of file diff --git a/include/message/arm_common.h b/include/message/arm_common.h new file mode 100644 index 0000000..e8e15b1 --- /dev/null +++ b/include/message/arm_common.h @@ -0,0 +1,150 @@ +#ifndef _UNITREE_ARM_ARM_COMMON_H_ +#define _UNITREE_ARM_ARM_COMMON_H_ + +#include + +#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_ \ No newline at end of file diff --git a/include/message/arm_motor_common.h b/include/message/arm_motor_common.h new file mode 100644 index 0000000..e248955 --- /dev/null +++ b/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 +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 \ No newline at end of file diff --git a/include/message/b1_common.h b/include/message/b1_common.h new file mode 100644 index 0000000..24debcd --- /dev/null +++ b/include/message/b1_common.h @@ -0,0 +1,211 @@ +#ifndef _UNITREE_ARM_B1_COMMON_H_ +#define _UNITREE_ARM_B1_COMMON_H_ + +#include +#include + +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 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 BQ_NTC; // x1 degrees centigrade + std::array MCU_NTC; // x1 degrees centigrade + std::array cell_vol; // cell voltage mV + } BmsState; + + typedef struct + { + float x; + float y; + float z; + } Cartesian; + + typedef struct + { + std::array quaternion; // quaternion, normalized, (w,x,y,z) + std::array gyroscope; // angular velocity （unit: rad/s) (raw data) + std::array accelerometer; // m/(s2) (raw data) + std::array 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 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 reserve; + } MotorCmd; // motor control + + typedef struct + { + std::array head; + uint8_t levelFlag; + uint8_t frameReserve; + + std::array SN; + std::array version; + uint16_t bandWidth; + IMU imu; + std::array motorState; + BmsState bms; + std::array footForce; // force sensors + std::array footForceEst; // force sensors + uint32_t tick; // reference real-time from motion controller (unit: us) + std::array wirelessRemote; // wireless commands + uint32_t reserve; + + uint32_t crc; + } LowState; // low level feedback + + typedef struct + { + std::array head; + uint8_t levelFlag; + uint8_t frameReserve; + + std::array SN; + std::array version; + uint16_t bandWidth; + std::array motorCmd; + BmsCmd bms; + std::array wirelessRemote; + uint32_t reserve; + + uint32_t crc; + } LowCmd; // low level control + + typedef struct + { + std::array head; + uint8_t levelFlag; + uint8_t frameReserve; + + std::array SN; + std::array version; + uint16_t bandWidth; + IMU imu; + std::array motorState; + BmsState bms; + std::array footForce; + std::array 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 position; // (unit: m), from own odometry in inertial frame, usually drift + float bodyHeight; // (unit: m, default: 0.28m), + std::array velocity; // (unit: m/s), forwardSpeed, sideSpeed, rotateSpeed in body frame + float yawSpeed; // (unit: rad/s), rotateSpeed in body frame + std::array rangeObstacle; + std::array footPosition2Body; // foot position relative to body + std::array footSpeed2Body; // foot speed relative to body + std::array wirelessRemote; + uint32_t reserve; + + uint32_t crc; + } HighState; // high level feedback + + typedef struct + { + std::array head; + uint8_t levelFlag; + uint8_t frameReserve; + + std::array SN; + std::array 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 position; // (unit: m), desired position in inertial frame + std::array euler; // (unit: rad), roll pitch yaw in stand mode + std::array velocity; // (unit: m/s), forwardSpeed, sideSpeed in body frame + float yawSpeed; // (unit: rad/s), rotateSpeed in body frame + BmsCmd bms; + std::array led; + std::array 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 \ No newline at end of file diff --git a/include/message/joystick_common.h b/include/message/joystick_common.h new file mode 100644 index 0000000..b9c1e06 --- /dev/null +++ b/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 + +// 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 \ No newline at end of file diff --git a/include/message/udp.h b/include/message/udp.h new file mode 100644 index 0000000..6b2c2a6 --- /dev/null +++ b/include/message/udp.h @@ -0,0 +1,69 @@ +#ifndef _UDP_H +#define _UDP_H + +#include +#include +#include +#include +#include +#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 &sendVec, std::vector &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 &sendVec, std::vector &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 \ No newline at end of file diff --git a/include/message/unitree_legged_msgs/Cartesian.h b/include/message/unitree_legged_msgs/Cartesian.h deleted file mode 100644 index 064a47e..0000000 --- a/include/message/unitree_legged_msgs/Cartesian.h +++ /dev/null @@ -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 -#include -#include - -#include -#include -#include -#include - - -namespace unitree_legged_msgs -{ -template -struct Cartesian_ -{ - typedef Cartesian_ 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_ > Ptr; - typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian_ const> ConstPtr; - -}; // struct Cartesian_ - -typedef ::unitree_legged_msgs::Cartesian_ > 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 -std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::Cartesian_ & v) -{ -ros::message_operations::Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, "", v); -return s; -} - - -template -bool operator==(const ::unitree_legged_msgs::Cartesian_ & lhs, const ::unitree_legged_msgs::Cartesian_ & rhs) -{ - return lhs.x == rhs.x && - lhs.y == rhs.y && - lhs.z == rhs.z; -} - -template -bool operator!=(const ::unitree_legged_msgs::Cartesian_ & lhs, const ::unitree_legged_msgs::Cartesian_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace unitree_legged_msgs - -namespace ros -{ -namespace message_traits -{ - - - - - -template -struct IsFixedSize< ::unitree_legged_msgs::Cartesian_ > - : TrueType - { }; - -template -struct IsFixedSize< ::unitree_legged_msgs::Cartesian_ const> - : TrueType - { }; - -template -struct IsMessage< ::unitree_legged_msgs::Cartesian_ > - : TrueType - { }; - -template -struct IsMessage< ::unitree_legged_msgs::Cartesian_ const> - : TrueType - { }; - -template -struct HasHeader< ::unitree_legged_msgs::Cartesian_ > - : FalseType - { }; - -template -struct HasHeader< ::unitree_legged_msgs::Cartesian_ const> - : FalseType - { }; - - -template -struct MD5Sum< ::unitree_legged_msgs::Cartesian_ > -{ - static const char* value() - { - return "cc153912f1453b708d221682bc23d9ac"; - } - - static const char* value(const ::unitree_legged_msgs::Cartesian_&) { return value(); } - static const uint64_t static_value1 = 0xcc153912f1453b70ULL; - static const uint64_t static_value2 = 0x8d221682bc23d9acULL; -}; - -template -struct DataType< ::unitree_legged_msgs::Cartesian_ > -{ - static const char* value() - { - return "unitree_legged_msgs/Cartesian"; - } - - static const char* value(const ::unitree_legged_msgs::Cartesian_&) { return value(); } -}; - -template -struct Definition< ::unitree_legged_msgs::Cartesian_ > -{ - static const char* value() - { - return "float32 x\n" -"float32 y\n" -"float32 z\n" -; - } - - static const char* value(const ::unitree_legged_msgs::Cartesian_&) { return value(); } -}; - -} // namespace message_traits -} // namespace ros - -namespace ros -{ -namespace serialization -{ - - template struct Serializer< ::unitree_legged_msgs::Cartesian_ > - { - template 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 -struct Printer< ::unitree_legged_msgs::Cartesian_ > -{ - template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::Cartesian_& v) - { - s << indent << "x: "; - Printer::stream(s, indent + " ", v.x); - s << indent << "y: "; - Printer::stream(s, indent + " ", v.y); - s << indent << "z: "; - Printer::stream(s, indent + " ", v.z); - } -}; - -} // namespace message_operations -} // namespace ros - -#endif // UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H diff --git a/include/message/unitree_legged_msgs/HighCmd.h b/include/message/unitree_legged_msgs/HighCmd.h deleted file mode 100644 index 985ac58..0000000 --- a/include/message/unitree_legged_msgs/HighCmd.h +++ /dev/null @@ -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 -#include -#include - -#include -#include -#include -#include - -#include - -namespace unitree_legged_msgs -{ -template -struct HighCmd_ -{ - typedef HighCmd_ 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_ (_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_ , 4> _led_type; - _led_type led; - - typedef boost::array _wirelessRemote_type; - _wirelessRemote_type wirelessRemote; - - typedef boost::array _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_ > Ptr; - typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd_ const> ConstPtr; - -}; // struct HighCmd_ - -typedef ::unitree_legged_msgs::HighCmd_ > 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 -std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::HighCmd_ & v) -{ -ros::message_operations::Printer< ::unitree_legged_msgs::HighCmd_ >::stream(s, "", v); -return s; -} - - -template -bool operator==(const ::unitree_legged_msgs::HighCmd_ & lhs, const ::unitree_legged_msgs::HighCmd_ & 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 -bool operator!=(const ::unitree_legged_msgs::HighCmd_ & lhs, const ::unitree_legged_msgs::HighCmd_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace unitree_legged_msgs - -namespace ros -{ -namespace message_traits -{ - - - - - -template -struct IsFixedSize< ::unitree_legged_msgs::HighCmd_ > - : TrueType - { }; - -template -struct IsFixedSize< ::unitree_legged_msgs::HighCmd_ const> - : TrueType - { }; - -template -struct IsMessage< ::unitree_legged_msgs::HighCmd_ > - : TrueType - { }; - -template -struct IsMessage< ::unitree_legged_msgs::HighCmd_ const> - : TrueType - { }; - -template -struct HasHeader< ::unitree_legged_msgs::HighCmd_ > - : FalseType - { }; - -template -struct HasHeader< ::unitree_legged_msgs::HighCmd_ const> - : FalseType - { }; - - -template -struct MD5Sum< ::unitree_legged_msgs::HighCmd_ > -{ - static const char* value() - { - return "1a655499a3f64905db59ceed65ca774a"; - } - - static const char* value(const ::unitree_legged_msgs::HighCmd_&) { return value(); } - static const uint64_t static_value1 = 0x1a655499a3f64905ULL; - static const uint64_t static_value2 = 0xdb59ceed65ca774aULL; -}; - -template -struct DataType< ::unitree_legged_msgs::HighCmd_ > -{ - static const char* value() - { - return "unitree_legged_msgs/HighCmd"; - } - - static const char* value(const ::unitree_legged_msgs::HighCmd_&) { return value(); } -}; - -template -struct Definition< ::unitree_legged_msgs::HighCmd_ > -{ - 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_&) { return value(); } -}; - -} // namespace message_traits -} // namespace ros - -namespace ros -{ -namespace serialization -{ - - template struct Serializer< ::unitree_legged_msgs::HighCmd_ > - { - template 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 -struct Printer< ::unitree_legged_msgs::HighCmd_ > -{ - template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::HighCmd_& v) - { - s << indent << "levelFlag: "; - Printer::stream(s, indent + " ", v.levelFlag); - s << indent << "commVersion: "; - Printer::stream(s, indent + " ", v.commVersion); - s << indent << "robotID: "; - Printer::stream(s, indent + " ", v.robotID); - s << indent << "SN: "; - Printer::stream(s, indent + " ", v.SN); - s << indent << "bandWidth: "; - Printer::stream(s, indent + " ", v.bandWidth); - s << indent << "mode: "; - Printer::stream(s, indent + " ", v.mode); - s << indent << "forwardSpeed: "; - Printer::stream(s, indent + " ", v.forwardSpeed); - s << indent << "sideSpeed: "; - Printer::stream(s, indent + " ", v.sideSpeed); - s << indent << "rotateSpeed: "; - Printer::stream(s, indent + " ", v.rotateSpeed); - s << indent << "bodyHeight: "; - Printer::stream(s, indent + " ", v.bodyHeight); - s << indent << "footRaiseHeight: "; - Printer::stream(s, indent + " ", v.footRaiseHeight); - s << indent << "yaw: "; - Printer::stream(s, indent + " ", v.yaw); - s << indent << "pitch: "; - Printer::stream(s, indent + " ", v.pitch); - s << indent << "roll: "; - Printer::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_ >::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::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::stream(s, indent + " ", v.AppRemote[i]); - } - s << indent << "reserve: "; - Printer::stream(s, indent + " ", v.reserve); - s << indent << "crc: "; - Printer::stream(s, indent + " ", v.crc); - } -}; - -} // namespace message_operations -} // namespace ros - -#endif // UNITREE_LEGGED_MSGS_MESSAGE_HIGHCMD_H diff --git a/include/message/unitree_legged_msgs/HighState.h b/include/message/unitree_legged_msgs/HighState.h deleted file mode 100644 index 5cadaf6..0000000 --- a/include/message/unitree_legged_msgs/HighState.h +++ /dev/null @@ -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 -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace unitree_legged_msgs -{ -template -struct HighState_ -{ - typedef HighState_ 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_ (_alloc)); - - footSpeed2Body.assign( ::unitree_legged_msgs::Cartesian_ (_alloc)); - - footForce.assign(0); - - footForceEst.assign(0); - - wirelessRemote.assign(0); - - eeForce.assign( ::unitree_legged_msgs::Cartesian_ (_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_ _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_ , 4> _footPosition2Body_type; - _footPosition2Body_type footPosition2Body; - - typedef boost::array< ::unitree_legged_msgs::Cartesian_ , 4> _footSpeed2Body_type; - _footSpeed2Body_type footSpeed2Body; - - typedef boost::array _footForce_type; - _footForce_type footForce; - - typedef boost::array _footForceEst_type; - _footForceEst_type footForceEst; - - typedef uint32_t _tick_type; - _tick_type tick; - - typedef boost::array _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_ , 4> _eeForce_type; - _eeForce_type eeForce; - - typedef boost::array _jointP_type; - _jointP_type jointP; - - - - - - typedef boost::shared_ptr< ::unitree_legged_msgs::HighState_ > Ptr; - typedef boost::shared_ptr< ::unitree_legged_msgs::HighState_ const> ConstPtr; - -}; // struct HighState_ - -typedef ::unitree_legged_msgs::HighState_ > 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 -std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::HighState_ & v) -{ -ros::message_operations::Printer< ::unitree_legged_msgs::HighState_ >::stream(s, "", v); -return s; -} - - -template -bool operator==(const ::unitree_legged_msgs::HighState_ & lhs, const ::unitree_legged_msgs::HighState_ & 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 -bool operator!=(const ::unitree_legged_msgs::HighState_ & lhs, const ::unitree_legged_msgs::HighState_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace unitree_legged_msgs - -namespace ros -{ -namespace message_traits -{ - - - - - -template -struct IsFixedSize< ::unitree_legged_msgs::HighState_ > - : TrueType - { }; - -template -struct IsFixedSize< ::unitree_legged_msgs::HighState_ const> - : TrueType - { }; - -template -struct IsMessage< ::unitree_legged_msgs::HighState_ > - : TrueType - { }; - -template -struct IsMessage< ::unitree_legged_msgs::HighState_ const> - : TrueType - { }; - -template -struct HasHeader< ::unitree_legged_msgs::HighState_ > - : FalseType - { }; - -template -struct HasHeader< ::unitree_legged_msgs::HighState_ const> - : FalseType - { }; - - -template -struct MD5Sum< ::unitree_legged_msgs::HighState_ > -{ - static const char* value() - { - return "a12e8b22df896c82203810ec6d521dad"; - } - - static const char* value(const ::unitree_legged_msgs::HighState_&) { return value(); } - static const uint64_t static_value1 = 0xa12e8b22df896c82ULL; - static const uint64_t static_value2 = 0x203810ec6d521dadULL; -}; - -template -struct DataType< ::unitree_legged_msgs::HighState_ > -{ - static const char* value() - { - return "unitree_legged_msgs/HighState"; - } - - static const char* value(const ::unitree_legged_msgs::HighState_&) { return value(); } -}; - -template -struct Definition< ::unitree_legged_msgs::HighState_ > -{ - 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_&) { return value(); } -}; - -} // namespace message_traits -} // namespace ros - -namespace ros -{ -namespace serialization -{ - - template struct Serializer< ::unitree_legged_msgs::HighState_ > - { - template 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 -struct Printer< ::unitree_legged_msgs::HighState_ > -{ - template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::HighState_& v) - { - s << indent << "levelFlag: "; - Printer::stream(s, indent + " ", v.levelFlag); - s << indent << "commVersion: "; - Printer::stream(s, indent + " ", v.commVersion); - s << indent << "robotID: "; - Printer::stream(s, indent + " ", v.robotID); - s << indent << "SN: "; - Printer::stream(s, indent + " ", v.SN); - s << indent << "bandWidth: "; - Printer::stream(s, indent + " ", v.bandWidth); - s << indent << "mode: "; - Printer::stream(s, indent + " ", v.mode); - s << indent << "imu: "; - s << std::endl; - Printer< ::unitree_legged_msgs::IMU_ >::stream(s, indent + " ", v.imu); - s << indent << "forwardSpeed: "; - Printer::stream(s, indent + " ", v.forwardSpeed); - s << indent << "sideSpeed: "; - Printer::stream(s, indent + " ", v.sideSpeed); - s << indent << "rotateSpeed: "; - Printer::stream(s, indent + " ", v.rotateSpeed); - s << indent << "bodyHeight: "; - Printer::stream(s, indent + " ", v.bodyHeight); - s << indent << "updownSpeed: "; - Printer::stream(s, indent + " ", v.updownSpeed); - s << indent << "forwardPosition: "; - Printer::stream(s, indent + " ", v.forwardPosition); - s << indent << "sidePosition: "; - Printer::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_ >::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_ >::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::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::stream(s, indent + " ", v.footForceEst[i]); - } - s << indent << "tick: "; - Printer::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::stream(s, indent + " ", v.wirelessRemote[i]); - } - s << indent << "reserve: "; - Printer::stream(s, indent + " ", v.reserve); - s << indent << "crc: "; - Printer::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_ >::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::stream(s, indent + " ", v.jointP[i]); - } - } -}; - -} // namespace message_operations -} // namespace ros - -#endif // UNITREE_LEGGED_MSGS_MESSAGE_HIGHSTATE_H diff --git a/include/message/unitree_legged_msgs/IMU.h b/include/message/unitree_legged_msgs/IMU.h deleted file mode 100644 index 0df8a22..0000000 --- a/include/message/unitree_legged_msgs/IMU.h +++ /dev/null @@ -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 -#include -#include - -#include -#include -#include -#include - - -namespace unitree_legged_msgs -{ -template -struct IMU_ -{ - typedef IMU_ 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 _quaternion_type; - _quaternion_type quaternion; - - typedef boost::array _gyroscope_type; - _gyroscope_type gyroscope; - - typedef boost::array _accelerometer_type; - _accelerometer_type accelerometer; - - typedef int8_t _temperature_type; - _temperature_type temperature; - - - - - - typedef boost::shared_ptr< ::unitree_legged_msgs::IMU_ > Ptr; - typedef boost::shared_ptr< ::unitree_legged_msgs::IMU_ const> ConstPtr; - -}; // struct IMU_ - -typedef ::unitree_legged_msgs::IMU_ > 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 -std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::IMU_ & v) -{ -ros::message_operations::Printer< ::unitree_legged_msgs::IMU_ >::stream(s, "", v); -return s; -} - - -template -bool operator==(const ::unitree_legged_msgs::IMU_ & lhs, const ::unitree_legged_msgs::IMU_ & rhs) -{ - return lhs.quaternion == rhs.quaternion && - lhs.gyroscope == rhs.gyroscope && - lhs.accelerometer == rhs.accelerometer && - lhs.temperature == rhs.temperature; -} - -template -bool operator!=(const ::unitree_legged_msgs::IMU_ & lhs, const ::unitree_legged_msgs::IMU_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace unitree_legged_msgs - -namespace ros -{ -namespace message_traits -{ - - - - - -template -struct IsFixedSize< ::unitree_legged_msgs::IMU_ > - : TrueType - { }; - -template -struct IsFixedSize< ::unitree_legged_msgs::IMU_ const> - : TrueType - { }; - -template -struct IsMessage< ::unitree_legged_msgs::IMU_ > - : TrueType - { }; - -template -struct IsMessage< ::unitree_legged_msgs::IMU_ const> - : TrueType - { }; - -template -struct HasHeader< ::unitree_legged_msgs::IMU_ > - : FalseType - { }; - -template -struct HasHeader< ::unitree_legged_msgs::IMU_ const> - : FalseType - { }; - - -template -struct MD5Sum< ::unitree_legged_msgs::IMU_ > -{ - static const char* value() - { - return "dd4bb4e42aa2f15aa1fb1b6a3c3752cb"; - } - - static const char* value(const ::unitree_legged_msgs::IMU_&) { return value(); } - static const uint64_t static_value1 = 0xdd4bb4e42aa2f15aULL; - static const uint64_t static_value2 = 0xa1fb1b6a3c3752cbULL; -}; - -template -struct DataType< ::unitree_legged_msgs::IMU_ > -{ - static const char* value() - { - return "unitree_legged_msgs/IMU"; - } - - static const char* value(const ::unitree_legged_msgs::IMU_&) { return value(); } -}; - -template -struct Definition< ::unitree_legged_msgs::IMU_ > -{ - 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_&) { return value(); } -}; - -} // namespace message_traits -} // namespace ros - -namespace ros -{ -namespace serialization -{ - - template struct Serializer< ::unitree_legged_msgs::IMU_ > - { - template 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 -struct Printer< ::unitree_legged_msgs::IMU_ > -{ - template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::IMU_& v) - { - s << indent << "quaternion[]" << std::endl; - for (size_t i = 0; i < v.quaternion.size(); ++i) - { - s << indent << " quaternion[" << i << "]: "; - Printer::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::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::stream(s, indent + " ", v.accelerometer[i]); - } - s << indent << "temperature: "; - Printer::stream(s, indent + " ", v.temperature); - } -}; - -} // namespace message_operations -} // namespace ros - -#endif // UNITREE_LEGGED_MSGS_MESSAGE_IMU_H diff --git a/include/message/unitree_legged_msgs/LED.h b/include/message/unitree_legged_msgs/LED.h deleted file mode 100644 index be563ab..0000000 --- a/include/message/unitree_legged_msgs/LED.h +++ /dev/null @@ -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 -#include -#include - -#include -#include -#include -#include - - -namespace unitree_legged_msgs -{ -template -struct LED_ -{ - typedef LED_ 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_ > Ptr; - typedef boost::shared_ptr< ::unitree_legged_msgs::LED_ const> ConstPtr; - -}; // struct LED_ - -typedef ::unitree_legged_msgs::LED_ > 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 -std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LED_ & v) -{ -ros::message_operations::Printer< ::unitree_legged_msgs::LED_ >::stream(s, "", v); -return s; -} - - -template -bool operator==(const ::unitree_legged_msgs::LED_ & lhs, const ::unitree_legged_msgs::LED_ & rhs) -{ - return lhs.r == rhs.r && - lhs.g == rhs.g && - lhs.b == rhs.b; -} - -template -bool operator!=(const ::unitree_legged_msgs::LED_ & lhs, const ::unitree_legged_msgs::LED_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace unitree_legged_msgs - -namespace ros -{ -namespace message_traits -{ - - - - - -template -struct IsFixedSize< ::unitree_legged_msgs::LED_ > - : TrueType - { }; - -template -struct IsFixedSize< ::unitree_legged_msgs::LED_ const> - : TrueType - { }; - -template -struct IsMessage< ::unitree_legged_msgs::LED_ > - : TrueType - { }; - -template -struct IsMessage< ::unitree_legged_msgs::LED_ const> - : TrueType - { }; - -template -struct HasHeader< ::unitree_legged_msgs::LED_ > - : FalseType - { }; - -template -struct HasHeader< ::unitree_legged_msgs::LED_ const> - : FalseType - { }; - - -template -struct MD5Sum< ::unitree_legged_msgs::LED_ > -{ - static const char* value() - { - return "353891e354491c51aabe32df673fb446"; - } - - static const char* value(const ::unitree_legged_msgs::LED_&) { return value(); } - static const uint64_t static_value1 = 0x353891e354491c51ULL; - static const uint64_t static_value2 = 0xaabe32df673fb446ULL; -}; - -template -struct DataType< ::unitree_legged_msgs::LED_ > -{ - static const char* value() - { - return "unitree_legged_msgs/LED"; - } - - static const char* value(const ::unitree_legged_msgs::LED_&) { return value(); } -}; - -template -struct Definition< ::unitree_legged_msgs::LED_ > -{ - static const char* value() - { - return "uint8 r\n" -"uint8 g\n" -"uint8 b\n" -; - } - - static const char* value(const ::unitree_legged_msgs::LED_&) { return value(); } -}; - -} // namespace message_traits -} // namespace ros - -namespace ros -{ -namespace serialization -{ - - template struct Serializer< ::unitree_legged_msgs::LED_ > - { - template 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 -struct Printer< ::unitree_legged_msgs::LED_ > -{ - template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LED_& v) - { - s << indent << "r: "; - Printer::stream(s, indent + " ", v.r); - s << indent << "g: "; - Printer::stream(s, indent + " ", v.g); - s << indent << "b: "; - Printer::stream(s, indent + " ", v.b); - } -}; - -} // namespace message_operations -} // namespace ros - -#endif // UNITREE_LEGGED_MSGS_MESSAGE_LED_H diff --git a/include/message/unitree_legged_msgs/LowCmd.h b/include/message/unitree_legged_msgs/LowCmd.h deleted file mode 100644 index 2d2f7c6..0000000 --- a/include/message/unitree_legged_msgs/LowCmd.h +++ /dev/null @@ -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 -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -namespace unitree_legged_msgs -{ -template -struct LowCmd_ -{ - typedef LowCmd_ 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_ (_alloc)); - - led.assign( ::unitree_legged_msgs::LED_ (_alloc)); - - wirelessRemote.assign(0); - - ff.assign( ::unitree_legged_msgs::Cartesian_ (_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_ , 20> _motorCmd_type; - _motorCmd_type motorCmd; - - typedef boost::array< ::unitree_legged_msgs::LED_ , 4> _led_type; - _led_type led; - - typedef boost::array _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_ , 4> _ff_type; - _ff_type ff; - - - - - - typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd_ > Ptr; - typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd_ const> ConstPtr; - -}; // struct LowCmd_ - -typedef ::unitree_legged_msgs::LowCmd_ > 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 -std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LowCmd_ & v) -{ -ros::message_operations::Printer< ::unitree_legged_msgs::LowCmd_ >::stream(s, "", v); -return s; -} - - -template -bool operator==(const ::unitree_legged_msgs::LowCmd_ & lhs, const ::unitree_legged_msgs::LowCmd_ & 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 -bool operator!=(const ::unitree_legged_msgs::LowCmd_ & lhs, const ::unitree_legged_msgs::LowCmd_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace unitree_legged_msgs - -namespace ros -{ -namespace message_traits -{ - - - - - -template -struct IsFixedSize< ::unitree_legged_msgs::LowCmd_ > - : TrueType - { }; - -template -struct IsFixedSize< ::unitree_legged_msgs::LowCmd_ const> - : TrueType - { }; - -template -struct IsMessage< ::unitree_legged_msgs::LowCmd_ > - : TrueType - { }; - -template -struct IsMessage< ::unitree_legged_msgs::LowCmd_ const> - : TrueType - { }; - -template -struct HasHeader< ::unitree_legged_msgs::LowCmd_ > - : FalseType - { }; - -template -struct HasHeader< ::unitree_legged_msgs::LowCmd_ const> - : FalseType - { }; - - -template -struct MD5Sum< ::unitree_legged_msgs::LowCmd_ > -{ - static const char* value() - { - return "357432b2562edd0a8e89b9c9f5fc4821"; - } - - static const char* value(const ::unitree_legged_msgs::LowCmd_&) { return value(); } - static const uint64_t static_value1 = 0x357432b2562edd0aULL; - static const uint64_t static_value2 = 0x8e89b9c9f5fc4821ULL; -}; - -template -struct DataType< ::unitree_legged_msgs::LowCmd_ > -{ - static const char* value() - { - return "unitree_legged_msgs/LowCmd"; - } - - static const char* value(const ::unitree_legged_msgs::LowCmd_&) { return value(); } -}; - -template -struct Definition< ::unitree_legged_msgs::LowCmd_ > -{ - 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_&) { return value(); } -}; - -} // namespace message_traits -} // namespace ros - -namespace ros -{ -namespace serialization -{ - - template struct Serializer< ::unitree_legged_msgs::LowCmd_ > - { - template 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 -struct Printer< ::unitree_legged_msgs::LowCmd_ > -{ - template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LowCmd_& v) - { - s << indent << "levelFlag: "; - Printer::stream(s, indent + " ", v.levelFlag); - s << indent << "commVersion: "; - Printer::stream(s, indent + " ", v.commVersion); - s << indent << "robotID: "; - Printer::stream(s, indent + " ", v.robotID); - s << indent << "SN: "; - Printer::stream(s, indent + " ", v.SN); - s << indent << "bandWidth: "; - Printer::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_ >::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_ >::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::stream(s, indent + " ", v.wirelessRemote[i]); - } - s << indent << "reserve: "; - Printer::stream(s, indent + " ", v.reserve); - s << indent << "crc: "; - Printer::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_ >::stream(s, indent + " ", v.ff[i]); - } - } -}; - -} // namespace message_operations -} // namespace ros - -#endif // UNITREE_LEGGED_MSGS_MESSAGE_LOWCMD_H diff --git a/include/message/unitree_legged_msgs/LowState.h b/include/message/unitree_legged_msgs/LowState.h deleted file mode 100644 index 118acda..0000000 --- a/include/message/unitree_legged_msgs/LowState.h +++ /dev/null @@ -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 -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace unitree_legged_msgs -{ -template -struct LowState_ -{ - typedef LowState_ 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_ (_alloc)); - - footForce.assign(0); - - footForceEst.assign(0); - - wirelessRemote.assign(0); - - eeForceRaw.assign( ::unitree_legged_msgs::Cartesian_ (_alloc)); - - eeForce.assign( ::unitree_legged_msgs::Cartesian_ (_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_ _imu_type; - _imu_type imu; - - typedef boost::array< ::unitree_legged_msgs::MotorState_ , 20> _motorState_type; - _motorState_type motorState; - - typedef boost::array _footForce_type; - _footForce_type footForce; - - typedef boost::array _footForceEst_type; - _footForceEst_type footForceEst; - - typedef uint32_t _tick_type; - _tick_type tick; - - typedef boost::array _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_ , 4> _eeForceRaw_type; - _eeForceRaw_type eeForceRaw; - - typedef boost::array< ::unitree_legged_msgs::Cartesian_ , 4> _eeForce_type; - _eeForce_type eeForce; - - typedef ::unitree_legged_msgs::Cartesian_ _position_type; - _position_type position; - - typedef ::unitree_legged_msgs::Cartesian_ _velocity_type; - _velocity_type velocity; - - typedef ::unitree_legged_msgs::Cartesian_ _velocity_w_type; - _velocity_w_type velocity_w; - - - - - - typedef boost::shared_ptr< ::unitree_legged_msgs::LowState_ > Ptr; - typedef boost::shared_ptr< ::unitree_legged_msgs::LowState_ const> ConstPtr; - -}; // struct LowState_ - -typedef ::unitree_legged_msgs::LowState_ > 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 -std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LowState_ & v) -{ -ros::message_operations::Printer< ::unitree_legged_msgs::LowState_ >::stream(s, "", v); -return s; -} - - -template -bool operator==(const ::unitree_legged_msgs::LowState_ & lhs, const ::unitree_legged_msgs::LowState_ & 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 -bool operator!=(const ::unitree_legged_msgs::LowState_ & lhs, const ::unitree_legged_msgs::LowState_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace unitree_legged_msgs - -namespace ros -{ -namespace message_traits -{ - - - - - -template -struct IsFixedSize< ::unitree_legged_msgs::LowState_ > - : TrueType - { }; - -template -struct IsFixedSize< ::unitree_legged_msgs::LowState_ const> - : TrueType - { }; - -template -struct IsMessage< ::unitree_legged_msgs::LowState_ > - : TrueType - { }; - -template -struct IsMessage< ::unitree_legged_msgs::LowState_ const> - : TrueType - { }; - -template -struct HasHeader< ::unitree_legged_msgs::LowState_ > - : FalseType - { }; - -template -struct HasHeader< ::unitree_legged_msgs::LowState_ const> - : FalseType - { }; - - -template -struct MD5Sum< ::unitree_legged_msgs::LowState_ > -{ - static const char* value() - { - return "cef9d4f6b5a89bd6330896affb1bca88"; - } - - static const char* value(const ::unitree_legged_msgs::LowState_&) { return value(); } - static const uint64_t static_value1 = 0xcef9d4f6b5a89bd6ULL; - static const uint64_t static_value2 = 0x330896affb1bca88ULL; -}; - -template -struct DataType< ::unitree_legged_msgs::LowState_ > -{ - static const char* value() - { - return "unitree_legged_msgs/LowState"; - } - - static const char* value(const ::unitree_legged_msgs::LowState_&) { return value(); } -}; - -template -struct Definition< ::unitree_legged_msgs::LowState_ > -{ - 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_&) { return value(); } -}; - -} // namespace message_traits -} // namespace ros - -namespace ros -{ -namespace serialization -{ - - template struct Serializer< ::unitree_legged_msgs::LowState_ > - { - template 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 -struct Printer< ::unitree_legged_msgs::LowState_ > -{ - template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LowState_& v) - { - s << indent << "levelFlag: "; - Printer::stream(s, indent + " ", v.levelFlag); - s << indent << "commVersion: "; - Printer::stream(s, indent + " ", v.commVersion); - s << indent << "robotID: "; - Printer::stream(s, indent + " ", v.robotID); - s << indent << "SN: "; - Printer::stream(s, indent + " ", v.SN); - s << indent << "bandWidth: "; - Printer::stream(s, indent + " ", v.bandWidth); - s << indent << "imu: "; - s << std::endl; - Printer< ::unitree_legged_msgs::IMU_ >::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_ >::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::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::stream(s, indent + " ", v.footForceEst[i]); - } - s << indent << "tick: "; - Printer::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::stream(s, indent + " ", v.wirelessRemote[i]); - } - s << indent << "reserve: "; - Printer::stream(s, indent + " ", v.reserve); - s << indent << "crc: "; - Printer::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_ >::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_ >::stream(s, indent + " ", v.eeForce[i]); - } - s << indent << "position: "; - s << std::endl; - Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.position); - s << indent << "velocity: "; - s << std::endl; - Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.velocity); - s << indent << "velocity_w: "; - s << std::endl; - Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.velocity_w); - } -}; - -} // namespace message_operations -} // namespace ros - -#endif // UNITREE_LEGGED_MSGS_MESSAGE_LOWSTATE_H diff --git a/include/model/ArmDynKineModel.h b/include/model/ArmDynKineModel.h deleted file mode 100755 index c82ad00..0000000 --- a/include/model/ArmDynKineModel.h +++ /dev/null @@ -1,124 +0,0 @@ - -#ifndef ARMDYNCKINEMODEL_H -#define ARMDYNCKINEMODEL_H - -#include -#include -#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 getJointQMax(){return _jointQMax;} - std::vector getJointQMin(){return _jointQMin;} - std::vector 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 _linkID; - - std::vector _body; - std::vector _linkMass; - std::vector _linkCom; - std::vector _linkInertia; - double _endEffectorMass; - Vec3 _endEffectorCom; - Mat3 _endEffectorInertia; - - std::vector _joint; - std::vector _jointAxis; - std::vector _jointPos; - std::vector _jointQMax; - std::vector _jointQMin; - std::vector _jointSpeedMax; - Vec3 _endPosGlobal, _endPosLocal; // based on mount pos - Vec3 _endMountPosGlobal, _endMountPosLocal; - std::vector _theta; - - /* inverse derivative kinematics */ - RigidBodyDynamics::Math::MatrixNd _Jacobian; - - /* inverse dynamics */ - std::vector _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 _linkPosLocal; - std::vector _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 G, CE, CI; - quadprogpp::Vector 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 \ No newline at end of file diff --git a/include/model/ArmModel.h b/include/model/ArmModel.h new file mode 100644 index 0000000..2f14bfe --- /dev/null +++ b/include/model/ArmModel.h @@ -0,0 +1,75 @@ +#ifndef ARMMODEL_H +#define ARMMODEL_H + +#include +#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 getJointQMax() {return _jointQMax;} + std::vector getJointQMin() {return _jointQMin;} + std::vector 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 _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 _Glist; + std::vector _jointAxis;// omega + std::vector _jointPos;// p_0 + std::vector _jointQMax; + std::vector _jointQMin; + std::vector _jointSpeedMax; + Vec3 _endPosLocal; // based on mount postion + Vec3 _endMountPosGlobal; + + std::vector _linkPosLocal; + std::vector _disJoint;//distance between joint + std::vector _linkMass; + std::vector _linkCom; // center of mass + std::vector _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 \ No newline at end of file diff --git a/include/model/ArmReal.h b/include/model/ArmReal.h deleted file mode 100755 index 831ce6a..0000000 --- a/include/model/ArmReal.h +++ /dev/null @@ -1,82 +0,0 @@ -#ifndef ARM_H -#define ARM_H - -#include -#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 jointKp); - void setJointKd(std::vector jointKd); - void setJointQ(std::vector jointQ); - void setJointDq(std::vector jointDq); - void setJointTau(std::vector jointTau); - - void getJointQ(std::vector &jointQState); - void getJointDq(std::vector &jointDqState); - void getJointDDq(std::vector &jointDDqState); - void getJointTau(std::vector &jointTauState); - - void getMotorQ(std::vector &motorQ); - void getMotorDq(std::vector &motorDq); - void getMotorTau(std::vector &motorTau); - - void armCalibration(); - void armCalibration(std::vector motorAngleBias); - bool armComm(); - size_t getDof(){return _dof;} - size_t getMotorNum(){return _motorNum;} - // int getMotorNum(){return _motorNum;} - -protected: - void _init(); - void _getCmd(std::vector &cmd); - void _setState(std::vector &motorState); - void _getMotorQBias(); - // void _setComm(); - void _setCaliBias(); - - int _motorNum; - int _dof; - int _commReturn; - bool _commYN, _motorCommYN; - std::vector _joints; - std::vector _jointCaliAngle; - std::vector _motorAngleBias; - std::vector _motorCmd; - std::vector _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 \ No newline at end of file diff --git a/include/model/Joint.h b/include/model/Joint.h deleted file mode 100755 index 9133718..0000000 --- a/include/model/Joint.h +++ /dev/null @@ -1,55 +0,0 @@ -#ifndef JOINT_H -#define JOINT_H - -#include -#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 &motorQ); - void getMotorDq(std::vector &motorDq); - void getMotorDDq(std::vector &motorDDq); - void getMotorTau(std::vector &motorTau); - - void getCmd(std::vector &cmd); - void setState(std::vector &state); - void setCaliBias(double cali, std::vector bias); - -protected: - JointMotorType _motorType; - int _motorNum; - double _jointInitAngle; - std::vector _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 \ No newline at end of file diff --git a/include/model/Motor.h b/include/model/Motor.h deleted file mode 100755 index cc32160..0000000 --- a/include/model/Motor.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef MOTOR_H -#define MOTOR_H - -#include -#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 &motorQ); - void getMotorDq(std::vector &motorDq); - void getMotorDDq(std::vector &motorDDq); - void getMotorTau(std::vector &motorTau); - - void setQBias(std::vector qBias); - void getCmd(std::vector &cmd); - void setState(std::vector &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 \ No newline at end of file diff --git a/include/trajectory/CartesionSpaceTraj.h b/include/trajectory/CartesionSpaceTraj.h deleted file mode 100755 index beb5bdb..0000000 --- a/include/trajectory/CartesionSpaceTraj.h +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef CartesionSpaceTraj_H -#define CartesionSpaceTraj_H - -#include -#include -#include -#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 _curveParam; - - /* MoveC instruct */ - Vec3 _center; - double _theta; - Vec3 _axis_x; - Vec3 _axis_y; - Vec3 _axis_z; -}; - -#endif // JOINTSPACETRAJ_H \ No newline at end of file diff --git a/include/trajectory/EndCircleTraj.h b/include/trajectory/EndCircleTraj.h index f4e8701..6724fa3 100755 --- a/include/trajectory/EndCircleTraj.h +++ b/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, - double maxSpeed); + void setEndRoundTraj(Vec6 startP, Vec6 middleP, Vec6 endP, double speed); private: void _centerCircle(Vec3 p1, Vec3 p2, Vec3 p3); bool _getEndTraj(HomoMat &homo, Vec6 &twist); - Vec3 _center; - double _radius; + Vec3 _center, _omegaAxis; + double _radius, _theta; + Vec6 _middlePosture; Vec6 _middleQ; - HomoMat _middleHomo; - HomoMat _centerHomo; - HomoMat _initHomoToCenter; + HomoMat _initHomoToCenter, _middleHomo, _centerHomo; RotMat _initOri; - double _maxSpeed, _goalAngle, _speed, _angle; - Vec3 _omegaAxis; bool _keepOrientation; }; diff --git a/include/trajectory/EndHomoTraj.h b/include/trajectory/EndHomoTraj.h index 6f29bd9..55f67c7 100755 --- a/include/trajectory/EndHomoTraj.h +++ b/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; - 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); + SCurve _sCurve; }; #endif // ENDHOMOTRAJ_H \ No newline at end of file diff --git a/include/trajectory/EndLineTraj.h b/include/trajectory/EndLineTraj.h index 0c8fc75..5573038 100755 --- a/include/trajectory/EndLineTraj.h +++ b/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); - void setEndLineTraj(std::string stateName, Vec3 deltaPos, Vec3 deltaOri, double maxMovingSpeed, double maxTurningSpeed); - void setEndLineTraj(Vec6 startP, Vec6 endP, double maxMovingSpeed, double maxTurningSpeed); - void setEndLineTraj(std::string startName, std::string endName, double maxMovingSpeed, double maxTurningSpeed); + bool setEndLineTraj(Vec6 startP, Vec6 endP, double speed); + bool setEndLineTraj(HomoMat startHomo, Vec3 deltaPos, Vec3 deltaOri, double speed); + bool setEndLineTraj(std::string stateName, Vec3 deltaPos, Vec3 deltaOri, double speed); + 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 _movingAxis, _turningAxis; - double _movingDistance, _turningAngle; - Vec3 _currentDistance, _currentAngle; - Vec3 _currentVelocity, _currentOmega; - - SCurve _posCurve; - SCurve _oriCurve; + Vec3 _currentDistance, _currentAngle, _omgtheta; + Mat3 _startR, _endR, _delatR, _currentR; + Vec3 _startp, _endp, _deltap, _currentp; }; #endif \ No newline at end of file diff --git a/include/trajectory/JointSpaceTraj.h b/include/trajectory/JointSpaceTraj.h index 3fc84e5..a63cce4 100755 --- a/include/trajectory/JointSpaceTraj.h +++ b/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); - - 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 stateQ, std::vector stateS, double pathTime); - void setJointTraj(std::vector stateName, std::vector stateS, double pathTime); - + bool setJointTraj(std::string startName, std::string endName, double speed); private: - void _checkAngleValid(const Vec6 &q, int pointOrder); - bool _checkJointAngleValid(const double &q, int jointOrder); - void _generateA345(); - SCurve _jointCurve; - int _jointNum; - 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 _curveParam; - + double ddQMax; + double dddQMax; }; #endif // JOINTSPACETRAJ_H \ No newline at end of file diff --git a/include/trajectory/SCurve.h b/include/trajectory/SCurve.h index 569adc4..cad0d30 100755 --- a/include/trajectory/SCurve.h +++ b/include/trajectory/SCurve.h @@ -3,7 +3,6 @@ /* 归一化后的s曲线 */ -#include #include class SCurve{ diff --git a/include/trajectory/StopForTime.h b/include/trajectory/StopForTime.h index 889c529..0e435ce 100755 --- a/include/trajectory/StopForTime.h +++ b/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 \ No newline at end of file diff --git a/include/trajectory/Trajectory.h b/include/trajectory/Trajectory.h index 108fab6..eaf40dc 100755 --- a/include/trajectory/Trajectory.h +++ b/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){ - _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; - } + Trajectory(CtrlComponents *ctrlComp); 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(){ - _pathStarted = false; - _reached = false; - _qPast = _startQ; - } - virtual void setGripper(double startQ, double endQ){ - _gripperStartQ = startQ; - _gripperEndQ = endQ; - } + void restart(); + virtual void setGripper(double startQ, double endQ, double speed = M_PI); + bool correctYN(){return _settingCorrect;} Vec6 getStartQ(){return _startQ;} Vec6 getEndQ(){return _endQ;} - double getEndGripperQ(){return _gripperEndQ;}; - double getStartGripperQ(){return _gripperStartQ;}; - HomoMat getStartHomo(){ - if(_hasKinematic){ - return _startHomo; - }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 getEndGripperQ(){return _endGripperQ;}; + double getStartGripperQ(){return _startGripperQ;}; + HomoMat getStartHomo(){return _startHomo;}; + HomoMat getEndHomo(){return _endHomo;}; + Vec6 getEndPosture(){return _endPosture;}; 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 _currentTime; - double _pathTime; - double _tCost; + double _startTime, _currentTime, _pathTime, _tCost; Vec6 _qPast; - Vec6 _startPosture, _endPosture; - Vec6 _startQ, _endQ; + Vec6 _startQ, _endQ, _deltaQ; HomoMat _startHomo, _endHomo; + Vec6 _startPosture, _endPosture, _deltaPosture; + double _startGripperQ, _endGripperQ; - double _gripperStartQ; - double _gripperEndQ; - - size_t _dof; - bool _hasKinematic; - - std::vector _jointMaxQ; - std::vector _jointMinQ; - std::vector _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 \ No newline at end of file diff --git a/include/trajectory/TrajectoryManager.h b/include/trajectory/TrajectoryManager.h index 9ba2e89..7bc364a 100755 --- a/include/trajectory/TrajectoryManager.h +++ b/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; diff --git a/lib/libUnitree_motor_SDK_Linux64.so b/lib/libUnitree_motor_SDK_Linux64.so deleted file mode 100755 index d118bc7..0000000 Binary files a/lib/libUnitree_motor_SDK_Linux64.so and /dev/null differ diff --git a/lib/libUnitree_motor_SDK_Linux64_EPOLL.so b/lib/libUnitree_motor_SDK_Linux64_EPOLL.so deleted file mode 100755 index 672e404..0000000 Binary files a/lib/libUnitree_motor_SDK_Linux64_EPOLL.so and /dev/null differ diff --git a/lib/libZ1_ROS_Linux64.so b/lib/libZ1_ROS_Linux64.so index bffbfc6..0e3c8dd 100644 Binary files a/lib/libZ1_ROS_Linux64.so and b/lib/libZ1_ROS_Linux64.so differ diff --git a/lib/libZ1_UDP_Linux64.so b/lib/libZ1_UDP_Linux64.so index 0ab20e8..cdbca9d 100644 Binary files a/lib/libZ1_UDP_Linux64.so and b/lib/libZ1_UDP_Linux64.so differ diff --git a/main.cpp b/main.cpp index 4138b19..2a474c0 100755 --- a/main.cpp +++ b/main.cpp @@ -1,38 +1,29 @@ -#include -#include #include #include -#include -#include "control/CtrlComponents.h" -#include "model/ArmDynKineModel.h" -#include "interface/IOUDP.h" - +#include "FSM/FiniteStateMachine.h" +#include "FSM/State_Passive.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_JointSpace.h" -#include "FSM/State_Passive.h" +#include "FSM/State_ToState.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< events; - CmdPanel *cmdPanel; CtrlComponents *ctrlComp = new CtrlComponents(); - if(ctrlComp->ctrl == Control::_SDK) - { - 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 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); + // control method + if(argc == 2) { + if(argv[1][0] == 'k'){ + ctrlComp->ctrl = Control::KEYBOARD; + }else if(argv[1][0] == 's'){ + ctrlComp->ctrl = Control::SDK; + }else if(argv[1][0] == 'j'){ + ctrlComp->ctrl = Control::JOYSTICK; + } } - -#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 states; - states.push_back(new State_Passive(ctrlComp)); // First state in states is the begining state for FSM + events.push_back(new ValueAction("q", "a", 0.5)); + 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 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 - ctrlComp->writeData(); -#endif - + delete fsm; delete ctrlComp; - return 0; -} +} \ No newline at end of file diff --git a/package.xml b/package.xml index 7d654fb..2c176ed 100644 --- a/package.xml +++ b/package.xml @@ -10,15 +10,12 @@ catkin roscp - rospy urdf xacro roscp - rospy urdf xacro roscp - rospy urdf xacro diff --git a/thirdparty/quadProgpp/src/Array.cc b/thirdparty/quadProgpp/src/Array.cc deleted file mode 100755 index 2b6eb7a..0000000 --- a/thirdparty/quadProgpp/src/Array.cc +++ /dev/null @@ -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 seq(unsigned int s, unsigned int e) -{ - std::set tmp; - for (unsigned int i = s; i <= e; i++) - tmp.insert(i); - - return tmp; -} - -std::set singleton(unsigned int i) -{ - std::set tmp; - tmp.insert(i); - - return tmp; -} - -} // namespace quadprogpp diff --git a/thirdparty/quadProgpp/src/QuadProg++.cc b/thirdparty/quadProgpp/src/QuadProg++.cc deleted file mode 100755 index f135c32..0000000 --- a/thirdparty/quadProgpp/src/QuadProg++.cc +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#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& d, const Matrix& J, const Vector& np); -void update_z(Vector& z, const Matrix& J, const Vector& d, int iq); -void update_r(const Matrix& R, Vector& r, const Vector& d, int iq); -bool add_constraint(Matrix& R, Matrix& J, Vector& d, unsigned int& iq, double& rnorm); -void delete_constraint(Matrix& R, Matrix& J, Vector& A, Vector& 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& A); -void cholesky_solve(const Matrix& L, Vector& x, const Vector& b); -void forward_elimination(const Matrix& L, Vector& y, const Vector& b); -void backward_elimination(const Matrix& U, Vector& x, const Vector& y); - -// Utility functions for computing the scalar product and the euclidean -// distance between two numbers -double scalar_product(const Vector& x, const Vector& y); -double distance(double a, double b); - -// Utility functions for printing vectors and matrices -void print_matrix(const char* name, const Matrix& A, int n = -1, int m = -1); - -template -void print_vector(const char* name, const Vector& v, int n = -1); - -// The Solving function, implementing the Goldfarb-Idnani method -double solve_quadprog(Matrix& G, Vector& g0, - const Matrix& CE, const Vector& ce0, - const Matrix& CI, const Vector& ci0, - Vector& 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 R(n, n), J(n, n); - Vector 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::has_infinity) - inf = std::numeric_limits::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 A(m + p), A_old(m + p), iai(m + p); - unsigned int iq, iter = 0; - Vector 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::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::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::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::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& d, const Matrix& J, const Vector& 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& z, const Matrix& J, const Vector& 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& R, Vector& r, const Vector& 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& R, Matrix& J, Vector& 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::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::epsilon() * R_norm) - { - // problem degenerate - return false; - } - R_norm = std::max(R_norm, fabs(d[iq - 1])); - return true; -} - -void delete_constraint(Matrix& R, Matrix& J, Vector& A, Vector& 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::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& x, const Vector& 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& 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& L, Vector& x, const Vector& b) -{ - int n = L.nrows(); - Vector 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& L, Vector& y, const Vector& 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& U, Vector& x, const Vector& 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& 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 -void print_vector(const char* name, const Vector& 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 diff --git a/thirdparty/tinyxml/src/tinystr.cpp b/thirdparty/tinyxml/src/tinystr.cpp deleted file mode 100644 index b04b081..0000000 --- a/thirdparty/tinyxml/src/tinystr.cpp +++ /dev/null @@ -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( 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( strlen(a) ); - tmp.reserve(a_len + b.length()); - tmp.append(a, a_len); - tmp += b; - return tmp; -} - - -#endif // TIXML_USE_STL diff --git a/thirdparty/tinyxml/src/tinyxml.cpp b/thirdparty/tinyxml/src/tinyxml.cpp deleted file mode 100644 index 5e24724..0000000 --- a/thirdparty/tinyxml/src/tinyxml.cpp +++ /dev/null @@ -1,1886 +0,0 @@ -/* -www.sourceforge.net/projects/tinyxml -Original code by 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 - -#ifdef TIXML_USE_STL -#include -#include -#endif - -#include "tinyxml/include/tinyxml.h" - -FILE* TiXmlFOpen( const char* filename, const char* mode ); - -bool TiXmlBase::condenseWhiteSpace = true; - -// Microsoft compiler security -FILE* TiXmlFOpen( const char* filename, const char* mode ) -{ - #if defined(_MSC_VER) && (_MSC_VER >= 1400 ) - FILE* fp = 0; - errno_t err = fopen_s( &fp, filename, mode ); - if ( !err && fp ) - return fp; - return 0; - #else - return fopen( filename, mode ); - #endif -} - -void TiXmlBase::EncodeString( const TIXML_STRING& str, TIXML_STRING* outString ) -{ - int i=0; - - while( i<(int)str.length() ) - { - unsigned char c = (unsigned char) str[i]; - - if ( c == '&' - && i < ( (int)str.length() - 2 ) - && str[i+1] == '#' - && str[i+2] == 'x' ) - { - // Hexadecimal character reference. - // Pass through unchanged. - // © -- copyright symbol, for example. - // - // The -1 is a bug fix from Rob Laveaux. It keeps - // an overflow from happening if there is no ';'. - // There are actually 2 ways to exit this loop - - // while fails (error case) and break (semicolon found). - // However, there is no mechanism (currently) for - // this function to return an error. - while ( i<(int)str.length()-1 ) - { - outString->append( str.c_str() + i, 1 ); - ++i; - if ( str[i] == ';' ) - break; - } - } - else if ( c == '&' ) - { - outString->append( entity[0].str, entity[0].strLength ); - ++i; - } - else if ( c == '<' ) - { - outString->append( entity[1].str, entity[1].strLength ); - ++i; - } - else if ( c == '>' ) - { - outString->append( entity[2].str, entity[2].strLength ); - ++i; - } - else if ( c == '\"' ) - { - outString->append( entity[3].str, entity[3].strLength ); - ++i; - } - else if ( c == '\'' ) - { - outString->append( entity[4].str, entity[4].strLength ); - ++i; - } - else if ( c < 32 ) - { - // Easy pass at non-alpha/numeric/symbol - // Below 32 is symbolic. - char buf[ 32 ]; - - #if defined(TIXML_SNPRINTF) - TIXML_SNPRINTF( buf, sizeof(buf), "&#x%02X;", (unsigned) ( c & 0xff ) ); - #else - sprintf( buf, "&#x%02X;", (unsigned) ( c & 0xff ) ); - #endif - - //*ME: warning C4267: convert 'size_t' to 'int' - //*ME: Int-Cast to make compiler happy ... - outString->append( buf, (int)strlen( buf ) ); - ++i; - } - else - { - //char realc = (char) c; - //outString->append( &realc, 1 ); - *outString += (char) c; // somewhat more efficient function call. - ++i; - } - } -} - - -TiXmlNode::TiXmlNode( NodeType _type ) : TiXmlBase() -{ - parent = 0; - type = _type; - firstChild = 0; - lastChild = 0; - prev = 0; - next = 0; -} - - -TiXmlNode::~TiXmlNode() -{ - TiXmlNode* node = firstChild; - TiXmlNode* temp = 0; - - while ( node ) - { - temp = node; - node = node->next; - delete temp; - } -} - - -void TiXmlNode::CopyTo( TiXmlNode* target ) const -{ - target->SetValue (value.c_str() ); - target->userData = userData; - target->location = location; -} - - -void TiXmlNode::Clear() -{ - TiXmlNode* node = firstChild; - TiXmlNode* temp = 0; - - while ( node ) - { - temp = node; - node = node->next; - delete temp; - } - - firstChild = 0; - lastChild = 0; -} - - -TiXmlNode* TiXmlNode::LinkEndChild( TiXmlNode* node ) -{ - assert( node->parent == 0 || node->parent == this ); - assert( node->GetDocument() == 0 || node->GetDocument() == this->GetDocument() ); - - if ( node->Type() == TiXmlNode::TINYXML_DOCUMENT ) - { - delete node; - if ( GetDocument() ) - GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - node->parent = this; - - node->prev = lastChild; - node->next = 0; - - if ( lastChild ) - lastChild->next = node; - else - firstChild = node; // it was an empty list. - - lastChild = node; - return node; -} - - -TiXmlNode* TiXmlNode::InsertEndChild( const TiXmlNode& addThis ) -{ - if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) - { - if ( GetDocument() ) - GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - TiXmlNode* node = addThis.Clone(); - if ( !node ) - return 0; - - return LinkEndChild( node ); -} - - -TiXmlNode* TiXmlNode::InsertBeforeChild( TiXmlNode* beforeThis, const TiXmlNode& addThis ) -{ - if ( !beforeThis || beforeThis->parent != this ) { - return 0; - } - if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) - { - if ( GetDocument() ) - GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - TiXmlNode* node = addThis.Clone(); - if ( !node ) - return 0; - node->parent = this; - - node->next = beforeThis; - node->prev = beforeThis->prev; - if ( beforeThis->prev ) - { - beforeThis->prev->next = node; - } - else - { - assert( firstChild == beforeThis ); - firstChild = node; - } - beforeThis->prev = node; - return node; -} - - -TiXmlNode* TiXmlNode::InsertAfterChild( TiXmlNode* afterThis, const TiXmlNode& addThis ) -{ - if ( !afterThis || afterThis->parent != this ) { - return 0; - } - if ( addThis.Type() == TiXmlNode::TINYXML_DOCUMENT ) - { - if ( GetDocument() ) - GetDocument()->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - TiXmlNode* node = addThis.Clone(); - if ( !node ) - return 0; - node->parent = this; - - node->prev = afterThis; - node->next = afterThis->next; - if ( afterThis->next ) - { - afterThis->next->prev = node; - } - else - { - assert( lastChild == afterThis ); - lastChild = node; - } - afterThis->next = node; - return node; -} - - -TiXmlNode* TiXmlNode::ReplaceChild( TiXmlNode* replaceThis, const TiXmlNode& withThis ) -{ - if ( !replaceThis ) - return 0; - - if ( replaceThis->parent != this ) - return 0; - - if ( withThis.ToDocument() ) { - // A document can never be a child. Thanks to Noam. - TiXmlDocument* document = GetDocument(); - if ( document ) - document->SetError( TIXML_ERROR_DOCUMENT_TOP_ONLY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - TiXmlNode* node = withThis.Clone(); - if ( !node ) - return 0; - - node->next = replaceThis->next; - node->prev = replaceThis->prev; - - if ( replaceThis->next ) - replaceThis->next->prev = node; - else - lastChild = node; - - if ( replaceThis->prev ) - replaceThis->prev->next = node; - else - firstChild = node; - - delete replaceThis; - node->parent = this; - return node; -} - - -bool TiXmlNode::RemoveChild( TiXmlNode* removeThis ) -{ - if ( !removeThis ) { - return false; - } - - if ( removeThis->parent != this ) - { - assert( 0 ); - return false; - } - - if ( removeThis->next ) - removeThis->next->prev = removeThis->prev; - else - lastChild = removeThis->prev; - - if ( removeThis->prev ) - removeThis->prev->next = removeThis->next; - else - firstChild = removeThis->next; - - delete removeThis; - return true; -} - -const TiXmlNode* TiXmlNode::FirstChild( const char * _value ) const -{ - const TiXmlNode* node; - for ( node = firstChild; node; node = node->next ) - { - if ( strcmp( node->Value(), _value ) == 0 ) - return node; - } - return 0; -} - - -const TiXmlNode* TiXmlNode::LastChild( const char * _value ) const -{ - const TiXmlNode* node; - for ( node = lastChild; node; node = node->prev ) - { - if ( strcmp( node->Value(), _value ) == 0 ) - return node; - } - return 0; -} - - -const TiXmlNode* TiXmlNode::IterateChildren( const TiXmlNode* previous ) const -{ - if ( !previous ) - { - return FirstChild(); - } - else - { - assert( previous->parent == this ); - return previous->NextSibling(); - } -} - - -const TiXmlNode* TiXmlNode::IterateChildren( const char * val, const TiXmlNode* previous ) const -{ - if ( !previous ) - { - return FirstChild( val ); - } - else - { - assert( previous->parent == this ); - return previous->NextSibling( val ); - } -} - - -const TiXmlNode* TiXmlNode::NextSibling( const char * _value ) const -{ - const TiXmlNode* node; - for ( node = next; node; node = node->next ) - { - if ( strcmp( node->Value(), _value ) == 0 ) - return node; - } - return 0; -} - - -const TiXmlNode* TiXmlNode::PreviousSibling( const char * _value ) const -{ - const TiXmlNode* node; - for ( node = prev; node; node = node->prev ) - { - if ( strcmp( node->Value(), _value ) == 0 ) - return node; - } - return 0; -} - - -void TiXmlElement::RemoveAttribute( const char * name ) -{ - #ifdef TIXML_USE_STL - TIXML_STRING str( name ); - TiXmlAttribute* node = attributeSet.Find( str ); - #else - TiXmlAttribute* node = attributeSet.Find( name ); - #endif - if ( node ) - { - attributeSet.Remove( node ); - delete node; - } -} - -const TiXmlElement* TiXmlNode::FirstChildElement() const -{ - const TiXmlNode* node; - - for ( node = FirstChild(); - node; - node = node->NextSibling() ) - { - if ( node->ToElement() ) - return node->ToElement(); - } - return 0; -} - - -const TiXmlElement* TiXmlNode::FirstChildElement( const char * _value ) const -{ - const TiXmlNode* node; - - for ( node = FirstChild( _value ); - node; - node = node->NextSibling( _value ) ) - { - if ( node->ToElement() ) - return node->ToElement(); - } - return 0; -} - - -const TiXmlElement* TiXmlNode::NextSiblingElement() const -{ - const TiXmlNode* node; - - for ( node = NextSibling(); - node; - node = node->NextSibling() ) - { - if ( node->ToElement() ) - return node->ToElement(); - } - return 0; -} - - -const TiXmlElement* TiXmlNode::NextSiblingElement( const char * _value ) const -{ - const TiXmlNode* node; - - for ( node = NextSibling( _value ); - node; - node = node->NextSibling( _value ) ) - { - if ( node->ToElement() ) - return node->ToElement(); - } - return 0; -} - - -const TiXmlDocument* TiXmlNode::GetDocument() const -{ - const TiXmlNode* node; - - for( node = this; node; node = node->parent ) - { - if ( node->ToDocument() ) - return node->ToDocument(); - } - return 0; -} - - -TiXmlElement::TiXmlElement (const char * _value) - : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) -{ - firstChild = lastChild = 0; - value = _value; -} - - -#ifdef TIXML_USE_STL -TiXmlElement::TiXmlElement( const std::string& _value ) - : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) -{ - firstChild = lastChild = 0; - value = _value; -} -#endif - - -TiXmlElement::TiXmlElement( const TiXmlElement& copy) - : TiXmlNode( TiXmlNode::TINYXML_ELEMENT ) -{ - firstChild = lastChild = 0; - copy.CopyTo( this ); -} - - -TiXmlElement& TiXmlElement::operator=( const TiXmlElement& base ) -{ - ClearThis(); - base.CopyTo( this ); - return *this; -} - - -TiXmlElement::~TiXmlElement() -{ - ClearThis(); -} - - -void TiXmlElement::ClearThis() -{ - Clear(); - while( attributeSet.First() ) - { - TiXmlAttribute* node = attributeSet.First(); - attributeSet.Remove( node ); - delete node; - } -} - - -const char* TiXmlElement::Attribute( const char* name ) const -{ - const TiXmlAttribute* node = attributeSet.Find( name ); - if ( node ) - return node->Value(); - return 0; -} - - -#ifdef TIXML_USE_STL -const std::string* TiXmlElement::Attribute( const std::string& name ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - if ( attrib ) - return &attrib->ValueStr(); - return 0; -} -#endif - - -const char* TiXmlElement::Attribute( const char* name, int* i ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - const char* result = 0; - - if ( attrib ) { - result = attrib->Value(); - if ( i ) { - attrib->QueryIntValue( i ); - } - } - return result; -} - - -#ifdef TIXML_USE_STL -const std::string* TiXmlElement::Attribute( const std::string& name, int* i ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - const std::string* result = 0; - - if ( attrib ) { - result = &attrib->ValueStr(); - if ( i ) { - attrib->QueryIntValue( i ); - } - } - return result; -} -#endif - - -const char* TiXmlElement::Attribute( const char* name, double* d ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - const char* result = 0; - - if ( attrib ) { - result = attrib->Value(); - if ( d ) { - attrib->QueryDoubleValue( d ); - } - } - return result; -} - - -#ifdef TIXML_USE_STL -const std::string* TiXmlElement::Attribute( const std::string& name, double* d ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - const std::string* result = 0; - - if ( attrib ) { - result = &attrib->ValueStr(); - if ( d ) { - attrib->QueryDoubleValue( d ); - } - } - return result; -} -#endif - - -int TiXmlElement::QueryIntAttribute( const char* name, int* ival ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - if ( !attrib ) - return TIXML_NO_ATTRIBUTE; - return attrib->QueryIntValue( ival ); -} - - -int TiXmlElement::QueryUnsignedAttribute( const char* name, unsigned* value ) const -{ - const TiXmlAttribute* node = attributeSet.Find( name ); - if ( !node ) - return TIXML_NO_ATTRIBUTE; - - int ival = 0; - int result = node->QueryIntValue( &ival ); - *value = (unsigned)ival; - return result; -} - - -int TiXmlElement::QueryBoolAttribute( const char* name, bool* bval ) const -{ - const TiXmlAttribute* node = attributeSet.Find( name ); - if ( !node ) - return TIXML_NO_ATTRIBUTE; - - int result = TIXML_WRONG_TYPE; - if ( StringEqual( node->Value(), "true", true, TIXML_ENCODING_UNKNOWN ) - || StringEqual( node->Value(), "yes", true, TIXML_ENCODING_UNKNOWN ) - || StringEqual( node->Value(), "1", true, TIXML_ENCODING_UNKNOWN ) ) - { - *bval = true; - result = TIXML_SUCCESS; - } - else if ( StringEqual( node->Value(), "false", true, TIXML_ENCODING_UNKNOWN ) - || StringEqual( node->Value(), "no", true, TIXML_ENCODING_UNKNOWN ) - || StringEqual( node->Value(), "0", true, TIXML_ENCODING_UNKNOWN ) ) - { - *bval = false; - result = TIXML_SUCCESS; - } - return result; -} - - - -#ifdef TIXML_USE_STL -int TiXmlElement::QueryIntAttribute( const std::string& name, int* ival ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - if ( !attrib ) - return TIXML_NO_ATTRIBUTE; - return attrib->QueryIntValue( ival ); -} -#endif - - -int TiXmlElement::QueryDoubleAttribute( const char* name, double* dval ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - if ( !attrib ) - return TIXML_NO_ATTRIBUTE; - return attrib->QueryDoubleValue( dval ); -} - - -#ifdef TIXML_USE_STL -int TiXmlElement::QueryDoubleAttribute( const std::string& name, double* dval ) const -{ - const TiXmlAttribute* attrib = attributeSet.Find( name ); - if ( !attrib ) - return TIXML_NO_ATTRIBUTE; - return attrib->QueryDoubleValue( dval ); -} -#endif - - -void TiXmlElement::SetAttribute( const char * name, int val ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); - if ( attrib ) { - attrib->SetIntValue( val ); - } -} - - -#ifdef TIXML_USE_STL -void TiXmlElement::SetAttribute( const std::string& name, int val ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); - if ( attrib ) { - attrib->SetIntValue( val ); - } -} -#endif - - -void TiXmlElement::SetDoubleAttribute( const char * name, double val ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); - if ( attrib ) { - attrib->SetDoubleValue( val ); - } -} - - -#ifdef TIXML_USE_STL -void TiXmlElement::SetDoubleAttribute( const std::string& name, double val ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( name ); - if ( attrib ) { - attrib->SetDoubleValue( val ); - } -} -#endif - - -void TiXmlElement::SetAttribute( const char * cname, const char * cvalue ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( cname ); - if ( attrib ) { - attrib->SetValue( cvalue ); - } -} - - -#ifdef TIXML_USE_STL -void TiXmlElement::SetAttribute( const std::string& _name, const std::string& _value ) -{ - TiXmlAttribute* attrib = attributeSet.FindOrCreate( _name ); - if ( attrib ) { - attrib->SetValue( _value ); - } -} -#endif - - -void TiXmlElement::Print( FILE* cfile, int depth ) const -{ - int i; - assert( cfile ); - for ( i=0; iNext() ) - { - fprintf( cfile, " " ); - attrib->Print( cfile, depth ); - } - - // There are 3 different formatting approaches: - // 1) An element without children is printed as a node - // 2) An element with only a text child is printed as text - // 3) An element with children is printed on multiple lines. - TiXmlNode* node; - if ( !firstChild ) - { - fprintf( cfile, " />" ); - } - else if ( firstChild == lastChild && firstChild->ToText() ) - { - fprintf( cfile, ">" ); - firstChild->Print( cfile, depth + 1 ); - fprintf( cfile, "", value.c_str() ); - } - else - { - fprintf( cfile, ">" ); - - for ( node = firstChild; node; node=node->NextSibling() ) - { - if ( !node->ToText() ) - { - fprintf( cfile, "\n" ); - } - node->Print( cfile, depth+1 ); - } - fprintf( cfile, "\n" ); - for( i=0; i", value.c_str() ); - } -} - - -void TiXmlElement::CopyTo( TiXmlElement* target ) const -{ - // superclass: - TiXmlNode::CopyTo( target ); - - // Element class: - // Clone the attributes, then clone the children. - const TiXmlAttribute* attribute = 0; - for( attribute = attributeSet.First(); - attribute; - attribute = attribute->Next() ) - { - target->SetAttribute( attribute->Name(), attribute->Value() ); - } - - TiXmlNode* node = 0; - for ( node = firstChild; node; node = node->NextSibling() ) - { - target->LinkEndChild( node->Clone() ); - } -} - -bool TiXmlElement::Accept( TiXmlVisitor* visitor ) const -{ - if ( visitor->VisitEnter( *this, attributeSet.First() ) ) - { - for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) - { - if ( !node->Accept( visitor ) ) - break; - } - } - return visitor->VisitExit( *this ); -} - - -TiXmlNode* TiXmlElement::Clone() const -{ - TiXmlElement* clone = new TiXmlElement( Value() ); - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -const char* TiXmlElement::GetText() const -{ - const TiXmlNode* child = this->FirstChild(); - if ( child ) { - const TiXmlText* childText = child->ToText(); - if ( childText ) { - return childText->Value(); - } - } - return 0; -} - - -TiXmlDocument::TiXmlDocument() : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) -{ - tabsize = 4; - useMicrosoftBOM = false; - ClearError(); -} - -TiXmlDocument::TiXmlDocument( const char * documentName ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) -{ - tabsize = 4; - useMicrosoftBOM = false; - value = documentName; - ClearError(); -} - - -#ifdef TIXML_USE_STL -TiXmlDocument::TiXmlDocument( const std::string& documentName ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) -{ - tabsize = 4; - useMicrosoftBOM = false; - value = documentName; - ClearError(); -} -#endif - - -TiXmlDocument::TiXmlDocument( const TiXmlDocument& copy ) : TiXmlNode( TiXmlNode::TINYXML_DOCUMENT ) -{ - copy.CopyTo( this ); -} - - -TiXmlDocument& TiXmlDocument::operator=( const TiXmlDocument& copy ) -{ - Clear(); - copy.CopyTo( this ); - return *this; -} - - -bool TiXmlDocument::LoadFile( TiXmlEncoding encoding ) -{ - return LoadFile( Value(), encoding ); -} - - -bool TiXmlDocument::SaveFile() const -{ - return SaveFile( Value() ); -} - -bool TiXmlDocument::LoadFile( const char* _filename, TiXmlEncoding encoding ) -{ - TIXML_STRING filename( _filename ); - value = filename; - - // reading in binary mode so that tinyxml can normalize the EOL - FILE* file = TiXmlFOpen( value.c_str (), "rb" ); - - if ( file ) - { - bool result = LoadFile( file, encoding ); - fclose( file ); - return result; - } - else - { - SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); - return false; - } -} - -bool TiXmlDocument::LoadFile( FILE* file, TiXmlEncoding encoding ) -{ - if ( !file ) - { - SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); - return false; - } - - // Delete the existing data: - Clear(); - location.Clear(); - - // Get the file size, so we can pre-allocate the string. HUGE speed impact. - long length = 0; - fseek( file, 0, SEEK_END ); - length = ftell( file ); - fseek( file, 0, SEEK_SET ); - - // Strange case, but good to handle up front. - if ( length <= 0 ) - { - SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return false; - } - - // Subtle bug here. TinyXml did use fgets. But from the XML spec: - // 2.11 End-of-Line Handling - // - // - // ...the XML processor MUST behave as if it normalized all line breaks in external - // parsed entities (including the document entity) on input, before parsing, by translating - // both the two-character sequence #xD #xA and any #xD that is not followed by #xA to - // a single #xA character. - // - // - // It is not clear fgets does that, and certainly isn't clear it works cross platform. - // Generally, you expect fgets to translate from the convention of the OS to the c/unix - // convention, and not work generally. - - /* - while( fgets( buf, sizeof(buf), file ) ) - { - data += buf; - } - */ - - char* buf = new char[ length+1 ]; - buf[0] = 0; - - if ( fread( buf, length, 1, file ) != 1 ) { - delete [] buf; - SetError( TIXML_ERROR_OPENING_FILE, 0, 0, TIXML_ENCODING_UNKNOWN ); - return false; - } - - // Process the buffer in place to normalize new lines. (See comment above.) - // Copies from the 'p' to 'q' pointer, where p can advance faster if - // a newline-carriage return is hit. - // - // Wikipedia: - // Systems based on ASCII or a compatible character set use either LF (Line feed, '\n', 0x0A, 10 in decimal) or - // CR (Carriage return, '\r', 0x0D, 13 in decimal) individually, or CR followed by LF (CR+LF, 0x0D 0x0A)... - // * LF: Multics, Unix and Unix-like systems (GNU/Linux, AIX, Xenix, Mac OS X, FreeBSD, etc.), BeOS, Amiga, RISC OS, and others - // * CR+LF: DEC RT-11 and most other early non-Unix, non-IBM OSes, CP/M, MP/M, DOS, OS/2, Microsoft Windows, Symbian OS - // * CR: Commodore 8-bit machines, Apple II family, Mac OS up to version 9 and OS-9 - - const char* p = buf; // the read head - char* q = buf; // the write head - const char CR = 0x0d; - const char LF = 0x0a; - - buf[length] = 0; - while( *p ) { - assert( p < (buf+length) ); - assert( q <= (buf+length) ); - assert( q <= p ); - - if ( *p == CR ) { - *q++ = LF; - p++; - if ( *p == LF ) { // check for CR+LF (and skip LF) - p++; - } - } - else { - *q++ = *p++; - } - } - assert( q <= (buf+length) ); - *q = 0; - - Parse( buf, 0, encoding ); - - delete [] buf; - return !Error(); -} - - -bool TiXmlDocument::SaveFile( const char * filename ) const -{ - // The old c stuff lives on... - FILE* fp = TiXmlFOpen( filename, "w" ); - if ( fp ) - { - bool result = SaveFile( fp ); - fclose( fp ); - return result; - } - return false; -} - - -bool TiXmlDocument::SaveFile( FILE* fp ) const -{ - if ( useMicrosoftBOM ) - { - const unsigned char TIXML_UTF_LEAD_0 = 0xefU; - const unsigned char TIXML_UTF_LEAD_1 = 0xbbU; - const unsigned char TIXML_UTF_LEAD_2 = 0xbfU; - - fputc( TIXML_UTF_LEAD_0, fp ); - fputc( TIXML_UTF_LEAD_1, fp ); - fputc( TIXML_UTF_LEAD_2, fp ); - } - Print( fp, 0 ); - return (ferror(fp) == 0); -} - - -void TiXmlDocument::CopyTo( TiXmlDocument* target ) const -{ - TiXmlNode::CopyTo( target ); - - target->error = error; - target->errorId = errorId; - target->errorDesc = errorDesc; - target->tabsize = tabsize; - target->errorLocation = errorLocation; - target->useMicrosoftBOM = useMicrosoftBOM; - - TiXmlNode* node = 0; - for ( node = firstChild; node; node = node->NextSibling() ) - { - target->LinkEndChild( node->Clone() ); - } -} - - -TiXmlNode* TiXmlDocument::Clone() const -{ - TiXmlDocument* clone = new TiXmlDocument(); - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -void TiXmlDocument::Print( FILE* cfile, int depth ) const -{ - assert( cfile ); - for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) - { - node->Print( cfile, depth ); - fprintf( cfile, "\n" ); - } -} - - -bool TiXmlDocument::Accept( TiXmlVisitor* visitor ) const -{ - if ( visitor->VisitEnter( *this ) ) - { - for ( const TiXmlNode* node=FirstChild(); node; node=node->NextSibling() ) - { - if ( !node->Accept( visitor ) ) - break; - } - } - return visitor->VisitExit( *this ); -} - - -const TiXmlAttribute* TiXmlAttribute::Next() const -{ - // We are using knowledge of the sentinel. The sentinel - // have a value or name. - if ( next->value.empty() && next->name.empty() ) - return 0; - return next; -} - -/* -TiXmlAttribute* TiXmlAttribute::Next() -{ - // We are using knowledge of the sentinel. The sentinel - // have a value or name. - if ( next->value.empty() && next->name.empty() ) - return 0; - return next; -} -*/ - -const TiXmlAttribute* TiXmlAttribute::Previous() const -{ - // We are using knowledge of the sentinel. The sentinel - // have a value or name. - if ( prev->value.empty() && prev->name.empty() ) - return 0; - return prev; -} - -/* -TiXmlAttribute* TiXmlAttribute::Previous() -{ - // We are using knowledge of the sentinel. The sentinel - // have a value or name. - if ( prev->value.empty() && prev->name.empty() ) - return 0; - return prev; -} -*/ - -void TiXmlAttribute::Print( FILE* cfile, int /*depth*/, TIXML_STRING* str ) const -{ - TIXML_STRING n, v; - - EncodeString( name, &n ); - EncodeString( value, &v ); - - if (value.find ('\"') == TIXML_STRING::npos) { - if ( cfile ) { - fprintf (cfile, "%s=\"%s\"", n.c_str(), v.c_str() ); - } - if ( str ) { - (*str) += n; (*str) += "=\""; (*str) += v; (*str) += "\""; - } - } - else { - if ( cfile ) { - fprintf (cfile, "%s='%s'", n.c_str(), v.c_str() ); - } - if ( str ) { - (*str) += n; (*str) += "='"; (*str) += v; (*str) += "'"; - } - } -} - - -int TiXmlAttribute::QueryIntValue( int* ival ) const -{ - if ( TIXML_SSCANF( value.c_str(), "%d", ival ) == 1 ) - return TIXML_SUCCESS; - return TIXML_WRONG_TYPE; -} - -int TiXmlAttribute::QueryDoubleValue( double* dval ) const -{ - if ( TIXML_SSCANF( value.c_str(), "%lf", dval ) == 1 ) - return TIXML_SUCCESS; - return TIXML_WRONG_TYPE; -} - -void TiXmlAttribute::SetIntValue( int _value ) -{ - char buf [64]; - #if defined(TIXML_SNPRINTF) - TIXML_SNPRINTF(buf, sizeof(buf), "%d", _value); - #else - sprintf (buf, "%d", _value); - #endif - SetValue (buf); -} - -void TiXmlAttribute::SetDoubleValue( double _value ) -{ - char buf [256]; - #if defined(TIXML_SNPRINTF) - TIXML_SNPRINTF( buf, sizeof(buf), "%g", _value); - #else - sprintf (buf, "%g", _value); - #endif - SetValue (buf); -} - -int TiXmlAttribute::IntValue() const -{ - return atoi (value.c_str ()); -} - -double TiXmlAttribute::DoubleValue() const -{ - return atof (value.c_str ()); -} - - -TiXmlComment::TiXmlComment( const TiXmlComment& copy ) : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) -{ - copy.CopyTo( this ); -} - - -TiXmlComment& TiXmlComment::operator=( const TiXmlComment& base ) -{ - Clear(); - base.CopyTo( this ); - return *this; -} - - -void TiXmlComment::Print( FILE* cfile, int depth ) const -{ - assert( cfile ); - for ( int i=0; i", value.c_str() ); -} - - -void TiXmlComment::CopyTo( TiXmlComment* target ) const -{ - TiXmlNode::CopyTo( target ); -} - - -bool TiXmlComment::Accept( TiXmlVisitor* visitor ) const -{ - return visitor->Visit( *this ); -} - - -TiXmlNode* TiXmlComment::Clone() const -{ - TiXmlComment* clone = new TiXmlComment(); - - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -void TiXmlText::Print( FILE* cfile, int depth ) const -{ - assert( cfile ); - if ( cdata ) - { - int i; - fprintf( cfile, "\n" ); - for ( i=0; i\n", value.c_str() ); // unformatted output - } - else - { - TIXML_STRING buffer; - EncodeString( value, &buffer ); - fprintf( cfile, "%s", buffer.c_str() ); - } -} - - -void TiXmlText::CopyTo( TiXmlText* target ) const -{ - TiXmlNode::CopyTo( target ); - target->cdata = cdata; -} - - -bool TiXmlText::Accept( TiXmlVisitor* visitor ) const -{ - return visitor->Visit( *this ); -} - - -TiXmlNode* TiXmlText::Clone() const -{ - TiXmlText* clone = 0; - clone = new TiXmlText( "" ); - - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -TiXmlDeclaration::TiXmlDeclaration( const char * _version, - const char * _encoding, - const char * _standalone ) - : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) -{ - version = _version; - encoding = _encoding; - standalone = _standalone; -} - - -#ifdef TIXML_USE_STL -TiXmlDeclaration::TiXmlDeclaration( const std::string& _version, - const std::string& _encoding, - const std::string& _standalone ) - : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) -{ - version = _version; - encoding = _encoding; - standalone = _standalone; -} -#endif - - -TiXmlDeclaration::TiXmlDeclaration( const TiXmlDeclaration& copy ) - : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) -{ - copy.CopyTo( this ); -} - - -TiXmlDeclaration& TiXmlDeclaration::operator=( const TiXmlDeclaration& copy ) -{ - Clear(); - copy.CopyTo( this ); - return *this; -} - - -void TiXmlDeclaration::Print( FILE* cfile, int /*depth*/, TIXML_STRING* str ) const -{ - if ( cfile ) fprintf( cfile, "" ); - if ( str ) (*str) += "?>"; -} - - -void TiXmlDeclaration::CopyTo( TiXmlDeclaration* target ) const -{ - TiXmlNode::CopyTo( target ); - - target->version = version; - target->encoding = encoding; - target->standalone = standalone; -} - - -bool TiXmlDeclaration::Accept( TiXmlVisitor* visitor ) const -{ - return visitor->Visit( *this ); -} - - -TiXmlNode* TiXmlDeclaration::Clone() const -{ - TiXmlDeclaration* clone = new TiXmlDeclaration(); - - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -void TiXmlUnknown::Print( FILE* cfile, int depth ) const -{ - for ( int i=0; i", value.c_str() ); -} - - -void TiXmlUnknown::CopyTo( TiXmlUnknown* target ) const -{ - TiXmlNode::CopyTo( target ); -} - - -bool TiXmlUnknown::Accept( TiXmlVisitor* visitor ) const -{ - return visitor->Visit( *this ); -} - - -TiXmlNode* TiXmlUnknown::Clone() const -{ - TiXmlUnknown* clone = new TiXmlUnknown(); - - if ( !clone ) - return 0; - - CopyTo( clone ); - return clone; -} - - -TiXmlAttributeSet::TiXmlAttributeSet() -{ - sentinel.next = &sentinel; - sentinel.prev = &sentinel; -} - - -TiXmlAttributeSet::~TiXmlAttributeSet() -{ - assert( sentinel.next == &sentinel ); - assert( sentinel.prev == &sentinel ); -} - - -void TiXmlAttributeSet::Add( TiXmlAttribute* addMe ) -{ - #ifdef TIXML_USE_STL - assert( !Find( TIXML_STRING( addMe->Name() ) ) ); // Shouldn't be multiply adding to the set. - #else - assert( !Find( addMe->Name() ) ); // Shouldn't be multiply adding to the set. - #endif - - addMe->next = &sentinel; - addMe->prev = sentinel.prev; - - sentinel.prev->next = addMe; - sentinel.prev = addMe; -} - -void TiXmlAttributeSet::Remove( TiXmlAttribute* removeMe ) -{ - TiXmlAttribute* node; - - for( node = sentinel.next; node != &sentinel; node = node->next ) - { - if ( node == removeMe ) - { - node->prev->next = node->next; - node->next->prev = node->prev; - node->next = 0; - node->prev = 0; - return; - } - } - assert( 0 ); // we tried to remove a non-linked attribute. -} - - -#ifdef TIXML_USE_STL -TiXmlAttribute* TiXmlAttributeSet::Find( const std::string& name ) const -{ - for( TiXmlAttribute* node = sentinel.next; node != &sentinel; node = node->next ) - { - if ( node->name == name ) - return node; - } - return 0; -} - -TiXmlAttribute* TiXmlAttributeSet::FindOrCreate( const std::string& _name ) -{ - TiXmlAttribute* attrib = Find( _name ); - if ( !attrib ) { - attrib = new TiXmlAttribute(); - Add( attrib ); - attrib->SetName( _name ); - } - return attrib; -} -#endif - - -TiXmlAttribute* TiXmlAttributeSet::Find( const char* name ) const -{ - for( TiXmlAttribute* node = sentinel.next; node != &sentinel; node = node->next ) - { - if ( strcmp( node->name.c_str(), name ) == 0 ) - return node; - } - return 0; -} - - -TiXmlAttribute* TiXmlAttributeSet::FindOrCreate( const char* _name ) -{ - TiXmlAttribute* attrib = Find( _name ); - if ( !attrib ) { - attrib = new TiXmlAttribute(); - Add( attrib ); - attrib->SetName( _name ); - } - return attrib; -} - - -#ifdef TIXML_USE_STL -std::istream& operator>> (std::istream & in, TiXmlNode & base) -{ - TIXML_STRING tag; - tag.reserve( 8 * 1000 ); - base.StreamIn( &in, &tag ); - - base.Parse( tag.c_str(), 0, TIXML_DEFAULT_ENCODING ); - return in; -} -#endif - - -#ifdef TIXML_USE_STL -std::ostream& operator<< (std::ostream & out, const TiXmlNode & base) -{ - TiXmlPrinter printer; - printer.SetStreamPrinting(); - base.Accept( &printer ); - out << printer.Str(); - - return out; -} - - -std::string& operator<< (std::string& out, const TiXmlNode& base ) -{ - TiXmlPrinter printer; - printer.SetStreamPrinting(); - base.Accept( &printer ); - out.append( printer.Str() ); - - return out; -} -#endif - - -TiXmlHandle TiXmlHandle::FirstChild() const -{ - if ( node ) - { - TiXmlNode* child = node->FirstChild(); - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::FirstChild( const char * value ) const -{ - if ( node ) - { - TiXmlNode* child = node->FirstChild( value ); - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::FirstChildElement() const -{ - if ( node ) - { - TiXmlElement* child = node->FirstChildElement(); - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::FirstChildElement( const char * value ) const -{ - if ( node ) - { - TiXmlElement* child = node->FirstChildElement( value ); - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::Child( int count ) const -{ - if ( node ) - { - int i; - TiXmlNode* child = node->FirstChild(); - for ( i=0; - child && iNextSibling(), ++i ) - { - // nothing - } - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::Child( const char* value, int count ) const -{ - if ( node ) - { - int i; - TiXmlNode* child = node->FirstChild( value ); - for ( i=0; - child && iNextSibling( value ), ++i ) - { - // nothing - } - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::ChildElement( int count ) const -{ - if ( node ) - { - int i; - TiXmlElement* child = node->FirstChildElement(); - for ( i=0; - child && iNextSiblingElement(), ++i ) - { - // nothing - } - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -TiXmlHandle TiXmlHandle::ChildElement( const char* value, int count ) const -{ - if ( node ) - { - int i; - TiXmlElement* child = node->FirstChildElement( value ); - for ( i=0; - child && iNextSiblingElement( value ), ++i ) - { - // nothing - } - if ( child ) - return TiXmlHandle( child ); - } - return TiXmlHandle( 0 ); -} - - -bool TiXmlPrinter::VisitEnter( const TiXmlDocument& ) -{ - return true; -} - -bool TiXmlPrinter::VisitExit( const TiXmlDocument& ) -{ - return true; -} - -bool TiXmlPrinter::VisitEnter( const TiXmlElement& element, const TiXmlAttribute* firstAttribute ) -{ - DoIndent(); - buffer += "<"; - buffer += element.Value(); - - for( const TiXmlAttribute* attrib = firstAttribute; attrib; attrib = attrib->Next() ) - { - buffer += " "; - attrib->Print( 0, 0, &buffer ); - } - - if ( !element.FirstChild() ) - { - buffer += " />"; - DoLineBreak(); - } - else - { - buffer += ">"; - if ( element.FirstChild()->ToText() - && element.LastChild() == element.FirstChild() - && element.FirstChild()->ToText()->CDATA() == false ) - { - simpleTextPrint = true; - // no DoLineBreak()! - } - else - { - DoLineBreak(); - } - } - ++depth; - return true; -} - - -bool TiXmlPrinter::VisitExit( const TiXmlElement& element ) -{ - --depth; - if ( !element.FirstChild() ) - { - // nothing. - } - else - { - if ( simpleTextPrint ) - { - simpleTextPrint = false; - } - else - { - DoIndent(); - } - buffer += ""; - DoLineBreak(); - } - return true; -} - - -bool TiXmlPrinter::Visit( const TiXmlText& text ) -{ - if ( text.CDATA() ) - { - DoIndent(); - buffer += ""; - DoLineBreak(); - } - else if ( simpleTextPrint ) - { - TIXML_STRING str; - TiXmlBase::EncodeString( text.ValueTStr(), &str ); - buffer += str; - } - else - { - DoIndent(); - TIXML_STRING str; - TiXmlBase::EncodeString( text.ValueTStr(), &str ); - buffer += str; - DoLineBreak(); - } - return true; -} - - -bool TiXmlPrinter::Visit( const TiXmlDeclaration& declaration ) -{ - DoIndent(); - declaration.Print( 0, 0, &buffer ); - DoLineBreak(); - return true; -} - - -bool TiXmlPrinter::Visit( const TiXmlComment& comment ) -{ - DoIndent(); - buffer += ""; - DoLineBreak(); - return true; -} - - -bool TiXmlPrinter::Visit( const TiXmlUnknown& unknown ) -{ - DoIndent(); - buffer += "<"; - buffer += unknown.Value(); - buffer += ">"; - DoLineBreak(); - return true; -} - diff --git a/thirdparty/tinyxml/src/tinyxmlerror.cpp b/thirdparty/tinyxml/src/tinyxmlerror.cpp deleted file mode 100644 index 84d0410..0000000 --- a/thirdparty/tinyxml/src/tinyxmlerror.cpp +++ /dev/null @@ -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.", -}; diff --git a/thirdparty/tinyxml/src/tinyxmlparser.cpp b/thirdparty/tinyxml/src/tinyxmlparser.cpp deleted file mode 100644 index 3f82f03..0000000 --- a/thirdparty/tinyxml/src/tinyxmlparser.cpp +++ /dev/null @@ -1,1638 +0,0 @@ -/* -www.sourceforge.net/projects/tinyxml -Original code by 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 -#include - -#include "tinyxml/include/tinyxml.h" - -//#define DEBUG_PARSER -#if defined( DEBUG_PARSER ) -# if defined( DEBUG ) && defined( _MSC_VER ) -# include -# define TIXML_LOG OutputDebugString -# else -# define TIXML_LOG printf -# endif -#endif - -// Note tha "PutString" hardcodes the same list. This -// is less flexible than it appears. Changing the entries -// or order will break putstring. -TiXmlBase::Entity TiXmlBase::entity[ TiXmlBase::NUM_ENTITY ] = -{ - { "&", 5, '&' }, - { "<", 4, '<' }, - { ">", 4, '>' }, - { """, 6, '\"' }, - { "'", 6, '\'' } -}; - -// Bunch of unicode info at: -// http://www.unicode.org/faq/utf_bom.html -// Including the basic of this table, which determines the #bytes in the -// sequence from the lead byte. 1 placed for invalid sequences -- -// although the result will be junk, pass it through as much as possible. -// Beware of the non-characters in UTF-8: -// ef bb bf (Microsoft "lead bytes") -// ef bf be -// ef bf bf - -const unsigned char TIXML_UTF_LEAD_0 = 0xefU; -const unsigned char TIXML_UTF_LEAD_1 = 0xbbU; -const unsigned char TIXML_UTF_LEAD_2 = 0xbfU; - -const int TiXmlBase::utf8ByteTable[256] = -{ - // 0 1 2 3 4 5 6 7 8 9 a b c d e f - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x00 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x10 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x20 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x30 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x40 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x50 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x60 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x70 End of ASCII range - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x80 0x80 to 0xc1 invalid - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0x90 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0xa0 - 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0xb0 - 1, 1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // 0xc0 0xc2 to 0xdf 2 byte - 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, // 0xd0 - 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, // 0xe0 0xe0 to 0xef 3 byte - 4, 4, 4, 4, 4, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // 0xf0 0xf0 to 0xf4 4 byte, 0xf5 and higher invalid -}; - - -void TiXmlBase::ConvertUTF32ToUTF8( unsigned long input, char* output, int* length ) -{ - const unsigned long BYTE_MASK = 0xBF; - const unsigned long BYTE_MARK = 0x80; - const unsigned long FIRST_BYTE_MARK[7] = { 0x00, 0x00, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC }; - - if (input < 0x80) - *length = 1; - else if ( input < 0x800 ) - *length = 2; - else if ( input < 0x10000 ) - *length = 3; - else if ( input < 0x200000 ) - *length = 4; - else - { *length = 0; return; } // This code won't covert this correctly anyway. - - output += *length; - - // Scary scary fall throughs. - switch (*length) - { - case 4: - --output; - *output = (char)((input | BYTE_MARK) & BYTE_MASK); - input >>= 6; - case 3: - --output; - *output = (char)((input | BYTE_MARK) & BYTE_MASK); - input >>= 6; - case 2: - --output; - *output = (char)((input | BYTE_MARK) & BYTE_MASK); - input >>= 6; - case 1: - --output; - *output = (char)(input | FIRST_BYTE_MARK[*length]); - } -} - - -/*static*/ int TiXmlBase::IsAlpha( unsigned char anyByte, TiXmlEncoding /*encoding*/ ) -{ - // This will only work for low-ascii, everything else is assumed to be a valid - // letter. I'm not sure this is the best approach, but it is quite tricky trying - // to figure out alhabetical vs. not across encoding. So take a very - // conservative approach. - -// if ( encoding == TIXML_ENCODING_UTF8 ) -// { - if ( anyByte < 127 ) - return isalpha( anyByte ); - else - return 1; // What else to do? The unicode set is huge...get the english ones right. -// } -// else -// { -// return isalpha( anyByte ); -// } -} - - -/*static*/ int TiXmlBase::IsAlphaNum( unsigned char anyByte, TiXmlEncoding /*encoding*/ ) -{ - // This will only work for low-ascii, everything else is assumed to be a valid - // letter. I'm not sure this is the best approach, but it is quite tricky trying - // to figure out alhabetical vs. not across encoding. So take a very - // conservative approach. - -// if ( encoding == TIXML_ENCODING_UTF8 ) -// { - if ( anyByte < 127 ) - return isalnum( anyByte ); - else - return 1; // What else to do? The unicode set is huge...get the english ones right. -// } -// else -// { -// return isalnum( anyByte ); -// } -} - - -class TiXmlParsingData -{ - friend class TiXmlDocument; - public: - void Stamp( const char* now, TiXmlEncoding encoding ); - - const TiXmlCursor& Cursor() const { return cursor; } - - private: - // Only used by the document! - TiXmlParsingData( const char* start, int _tabsize, int row, int col ) - { - assert( start ); - stamp = start; - tabsize = _tabsize; - cursor.row = row; - cursor.col = col; - } - - TiXmlCursor cursor; - const char* stamp; - int tabsize; -}; - - -void TiXmlParsingData::Stamp( const char* now, TiXmlEncoding encoding ) -{ - assert( now ); - - // Do nothing if the tabsize is 0. - if ( tabsize < 1 ) - { - return; - } - - // Get the current row, column. - int row = cursor.row; - int col = cursor.col; - const char* p = stamp; - assert( p ); - - while ( p < now ) - { - // Treat p as unsigned, so we have a happy compiler. - const unsigned char* pU = (const unsigned char*)p; - - // Code contributed by Fletcher Dunn: (modified by lee) - switch (*pU) { - case 0: - // We *should* never get here, but in case we do, don't - // advance past the terminating null character, ever - return; - - case '\r': - // bump down to the next line - ++row; - col = 0; - // Eat the character - ++p; - - // Check for \r\n sequence, and treat this as a single character - if (*p == '\n') { - ++p; - } - break; - - case '\n': - // bump down to the next line - ++row; - col = 0; - - // Eat the character - ++p; - - // Check for \n\r sequence, and treat this as a single - // character. (Yes, this bizarre thing does occur still - // on some arcane platforms...) - if (*p == '\r') { - ++p; - } - break; - - case '\t': - // Eat the character - ++p; - - // Skip to next tab stop - col = (col / tabsize + 1) * tabsize; - break; - - case TIXML_UTF_LEAD_0: - if ( encoding == TIXML_ENCODING_UTF8 ) - { - if ( *(p+1) && *(p+2) ) - { - // In these cases, don't advance the column. These are - // 0-width spaces. - if ( *(pU+1)==TIXML_UTF_LEAD_1 && *(pU+2)==TIXML_UTF_LEAD_2 ) - p += 3; - else if ( *(pU+1)==0xbfU && *(pU+2)==0xbeU ) - p += 3; - else if ( *(pU+1)==0xbfU && *(pU+2)==0xbfU ) - p += 3; - else - { p +=3; ++col; } // A normal character. - } - } - else - { - ++p; - ++col; - } - break; - - default: - if ( encoding == TIXML_ENCODING_UTF8 ) - { - // Eat the 1 to 4 byte utf8 character. - int step = TiXmlBase::utf8ByteTable[*((const unsigned char*)p)]; - if ( step == 0 ) - step = 1; // Error case from bad encoding, but handle gracefully. - p += step; - - // Just advance one column, of course. - ++col; - } - else - { - ++p; - ++col; - } - break; - } - } - cursor.row = row; - cursor.col = col; - assert( cursor.row >= -1 ); - assert( cursor.col >= -1 ); - stamp = p; - assert( stamp ); -} - - -const char* TiXmlBase::SkipWhiteSpace( const char* p, TiXmlEncoding encoding ) -{ - if ( !p || !*p ) - { - return 0; - } - if ( encoding == TIXML_ENCODING_UTF8 ) - { - while ( *p ) - { - const unsigned char* pU = (const unsigned char*)p; - - // Skip the stupid Microsoft UTF-8 Byte order marks - if ( *(pU+0)==TIXML_UTF_LEAD_0 - && *(pU+1)==TIXML_UTF_LEAD_1 - && *(pU+2)==TIXML_UTF_LEAD_2 ) - { - p += 3; - continue; - } - else if(*(pU+0)==TIXML_UTF_LEAD_0 - && *(pU+1)==0xbfU - && *(pU+2)==0xbeU ) - { - p += 3; - continue; - } - else if(*(pU+0)==TIXML_UTF_LEAD_0 - && *(pU+1)==0xbfU - && *(pU+2)==0xbfU ) - { - p += 3; - continue; - } - - if ( IsWhiteSpace( *p ) ) // Still using old rules for white space. - ++p; - else - break; - } - } - else - { - while ( *p && IsWhiteSpace( *p ) ) - ++p; - } - - return p; -} - -#ifdef TIXML_USE_STL -/*static*/ bool TiXmlBase::StreamWhiteSpace( std::istream * in, TIXML_STRING * tag ) -{ - for( ;; ) - { - if ( !in->good() ) return false; - - int c = in->peek(); - // At this scope, we can't get to a document. So fail silently. - if ( !IsWhiteSpace( c ) || c <= 0 ) - return true; - - *tag += (char) in->get(); - } -} - -/*static*/ bool TiXmlBase::StreamTo( std::istream * in, int character, TIXML_STRING * tag ) -{ - //assert( character > 0 && character < 128 ); // else it won't work in utf-8 - while ( in->good() ) - { - int c = in->peek(); - if ( c == character ) - return true; - if ( c <= 0 ) // Silent failure: can't get document at this scope - return false; - - in->get(); - *tag += (char) c; - } - return false; -} -#endif - -// One of TinyXML's more performance demanding functions. Try to keep the memory overhead down. The -// "assign" optimization removes over 10% of the execution time. -// -const char* TiXmlBase::ReadName( const char* p, TIXML_STRING * name, TiXmlEncoding encoding ) -{ - // Oddly, not supported on some comilers, - //name->clear(); - // So use this: - *name = ""; - assert( p ); - - // Names start with letters or underscores. - // Of course, in unicode, tinyxml has no idea what a letter *is*. The - // algorithm is generous. - // - // After that, they can be letters, underscores, numbers, - // hyphens, or colons. (Colons are valid ony for namespaces, - // but tinyxml can't tell namespaces from names.) - if ( p && *p - && ( IsAlpha( (unsigned char) *p, encoding ) || *p == '_' ) ) - { - const char* start = p; - while( p && *p - && ( IsAlphaNum( (unsigned char ) *p, encoding ) - || *p == '_' - || *p == '-' - || *p == '.' - || *p == ':' ) ) - { - //(*name) += *p; // expensive - ++p; - } - if ( p-start > 0 ) { - name->assign( start, p-start ); - } - return p; - } - return 0; -} - -const char* TiXmlBase::GetEntity( const char* p, char* value, int* length, TiXmlEncoding encoding ) -{ - // Presume an entity, and pull it out. - TIXML_STRING ent; - int i; - *length = 0; - - if ( *(p+1) && *(p+1) == '#' && *(p+2) ) - { - unsigned long ucs = 0; - ptrdiff_t delta = 0; - unsigned mult = 1; - - if ( *(p+2) == 'x' ) - { - // Hexadecimal. - if ( !*(p+3) ) return 0; - - const char* q = p+3; - q = strchr( q, ';' ); - - if ( !q || !*q ) return 0; - - delta = q-p; - --q; - - while ( *q != 'x' ) - { - if ( *q >= '0' && *q <= '9' ) - ucs += mult * (*q - '0'); - else if ( *q >= 'a' && *q <= 'f' ) - ucs += mult * (*q - 'a' + 10); - else if ( *q >= 'A' && *q <= 'F' ) - ucs += mult * (*q - 'A' + 10 ); - else - return 0; - mult *= 16; - --q; - } - } - else - { - // Decimal. - if ( !*(p+2) ) return 0; - - const char* q = p+2; - q = strchr( q, ';' ); - - if ( !q || !*q ) return 0; - - delta = q-p; - --q; - - while ( *q != '#' ) - { - if ( *q >= '0' && *q <= '9' ) - ucs += mult * (*q - '0'); - else - return 0; - mult *= 10; - --q; - } - } - if ( encoding == TIXML_ENCODING_UTF8 ) - { - // convert the UCS to UTF-8 - ConvertUTF32ToUTF8( ucs, value, length ); - } - else - { - *value = (char)ucs; - *length = 1; - } - return p + delta + 1; - } - - // Now try to match it. - for( i=0; iappend( cArr, len ); - } - } - else - { - bool whitespace = false; - - // Remove leading white space: - p = SkipWhiteSpace( p, encoding ); - while ( p && *p - && !StringEqual( p, endTag, caseInsensitive, encoding ) ) - { - if ( *p == '\r' || *p == '\n' ) - { - whitespace = true; - ++p; - } - else if ( IsWhiteSpace( *p ) ) - { - whitespace = true; - ++p; - } - else - { - // If we've found whitespace, add it before the - // new character. Any whitespace just becomes a space. - if ( whitespace ) - { - (*text) += ' '; - whitespace = false; - } - int len; - char cArr[4] = { 0, 0, 0, 0 }; - p = GetChar( p, cArr, &len, encoding ); - if ( len == 1 ) - (*text) += cArr[0]; // more efficient - else - text->append( cArr, len ); - } - } - } - if ( p && *p ) - p += strlen( endTag ); - return ( p && *p ) ? p : 0; -} - -#ifdef TIXML_USE_STL - -void TiXmlDocument::StreamIn( std::istream * in, TIXML_STRING * tag ) -{ - // The basic issue with a document is that we don't know what we're - // streaming. Read something presumed to be a tag (and hope), then - // identify it, and call the appropriate stream method on the tag. - // - // This "pre-streaming" will never read the closing ">" so the - // sub-tag can orient itself. - - if ( !StreamTo( in, '<', tag ) ) - { - SetError( TIXML_ERROR_PARSING_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return; - } - - while ( in->good() ) - { - int tagIndex = (int) tag->length(); - while ( in->good() && in->peek() != '>' ) - { - int c = in->get(); - if ( c <= 0 ) - { - SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); - break; - } - (*tag) += (char) c; - } - - if ( in->good() ) - { - // We now have something we presume to be a node of - // some sort. Identify it, and call the node to - // continue streaming. - TiXmlNode* node = Identify( tag->c_str() + tagIndex, TIXML_DEFAULT_ENCODING ); - - if ( node ) - { - node->StreamIn( in, tag ); - bool isElement = node->ToElement() != 0; - delete node; - node = 0; - - // If this is the root element, we're done. Parsing will be - // done by the >> operator. - if ( isElement ) - { - return; - } - } - else - { - SetError( TIXML_ERROR, 0, 0, TIXML_ENCODING_UNKNOWN ); - return; - } - } - } - // We should have returned sooner. - SetError( TIXML_ERROR, 0, 0, TIXML_ENCODING_UNKNOWN ); -} - -#endif - -const char* TiXmlDocument::Parse( const char* p, TiXmlParsingData* prevData, TiXmlEncoding encoding ) -{ - ClearError(); - - // Parse away, at the document level. Since a document - // contains nothing but other tags, most of what happens - // here is skipping white space. - if ( !p || !*p ) - { - SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - // Note that, for a document, this needs to come - // before the while space skip, so that parsing - // starts from the pointer we are given. - location.Clear(); - if ( prevData ) - { - location.row = prevData->cursor.row; - location.col = prevData->cursor.col; - } - else - { - location.row = 0; - location.col = 0; - } - TiXmlParsingData data( p, TabSize(), location.row, location.col ); - location = data.Cursor(); - - if ( encoding == TIXML_ENCODING_UNKNOWN ) - { - // Check for the Microsoft UTF-8 lead bytes. - const unsigned char* pU = (const unsigned char*)p; - if ( *(pU+0) && *(pU+0) == TIXML_UTF_LEAD_0 - && *(pU+1) && *(pU+1) == TIXML_UTF_LEAD_1 - && *(pU+2) && *(pU+2) == TIXML_UTF_LEAD_2 ) - { - encoding = TIXML_ENCODING_UTF8; - useMicrosoftBOM = true; - } - } - - p = SkipWhiteSpace( p, encoding ); - if ( !p ) - { - SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, TIXML_ENCODING_UNKNOWN ); - return 0; - } - - while ( p && *p ) - { - TiXmlNode* node = Identify( p, encoding ); - if ( node ) - { - p = node->Parse( p, &data, encoding ); - LinkEndChild( node ); - } - else - { - break; - } - - // Did we get encoding info? - if ( encoding == TIXML_ENCODING_UNKNOWN - && node->ToDeclaration() ) - { - TiXmlDeclaration* dec = node->ToDeclaration(); - const char* enc = dec->Encoding(); - assert( enc ); - - if ( *enc == 0 ) - encoding = TIXML_ENCODING_UTF8; - else if ( StringEqual( enc, "UTF-8", true, TIXML_ENCODING_UNKNOWN ) ) - encoding = TIXML_ENCODING_UTF8; - else if ( StringEqual( enc, "UTF8", true, TIXML_ENCODING_UNKNOWN ) ) - encoding = TIXML_ENCODING_UTF8; // incorrect, but be nice - else - encoding = TIXML_ENCODING_LEGACY; - } - - p = SkipWhiteSpace( p, encoding ); - } - - // Was this empty? - if ( !firstChild ) { - SetError( TIXML_ERROR_DOCUMENT_EMPTY, 0, 0, encoding ); - return 0; - } - - // All is well. - return p; -} - -void TiXmlDocument::SetError( int err, const char* pError, TiXmlParsingData* data, TiXmlEncoding encoding ) -{ - // The first error in a chain is more accurate - don't set again! - if ( error ) - return; - - assert( err > 0 && err < TIXML_ERROR_STRING_COUNT ); - error = true; - errorId = err; - errorDesc = errorString[ errorId ]; - - errorLocation.Clear(); - if ( pError && data ) - { - data->Stamp( pError, encoding ); - errorLocation = data->Cursor(); - } -} - - -TiXmlNode* TiXmlNode::Identify( const char* p, TiXmlEncoding encoding ) -{ - TiXmlNode* returnNode = 0; - - p = SkipWhiteSpace( p, encoding ); - if( !p || !*p || *p != '<' ) - { - return 0; - } - - p = SkipWhiteSpace( p, encoding ); - - if ( !p || !*p ) - { - return 0; - } - - // What is this thing? - // - Elements start with a letter or underscore, but xml is reserved. - // - Comments: "; - - if ( !StringEqual( p, startTag, false, encoding ) ) - { - if ( document ) - document->SetError( TIXML_ERROR_PARSING_COMMENT, p, data, encoding ); - return 0; - } - p += strlen( startTag ); - - // [ 1475201 ] TinyXML parses entities in comments - // Oops - ReadText doesn't work, because we don't want to parse the entities. - // p = ReadText( p, &value, false, endTag, false, encoding ); - // - // from the XML spec: - /* - [Definition: Comments may appear anywhere in a document outside other markup; in addition, - they may appear within the document type declaration at places allowed by the grammar. - They are not part of the document's character data; an XML processor MAY, but need not, - make it possible for an application to retrieve the text of comments. For compatibility, - the string "--" (double-hyphen) MUST NOT occur within comments.] Parameter entity - references MUST NOT be recognized within comments. - - An example of a comment: - - - */ - - value = ""; - // Keep all the white space. - while ( p && *p && !StringEqual( p, endTag, false, encoding ) ) - { - value.append( p, 1 ); - ++p; - } - if ( p && *p ) - p += strlen( endTag ); - - return p; -} - - -const char* TiXmlAttribute::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ) -{ - p = SkipWhiteSpace( p, encoding ); - if ( !p || !*p ) return 0; - - if ( data ) - { - data->Stamp( p, encoding ); - location = data->Cursor(); - } - // Read the name, the '=' and the value. - const char* pErr = p; - p = ReadName( p, &name, encoding ); - if ( !p || !*p ) - { - if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, pErr, data, encoding ); - return 0; - } - p = SkipWhiteSpace( p, encoding ); - if ( !p || !*p || *p != '=' ) - { - if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); - return 0; - } - - ++p; // skip '=' - p = SkipWhiteSpace( p, encoding ); - if ( !p || !*p ) - { - if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); - return 0; - } - - const char* end; - const char SINGLE_QUOTE = '\''; - const char DOUBLE_QUOTE = '\"'; - - if ( *p == SINGLE_QUOTE ) - { - ++p; - end = "\'"; // single quote in string - p = ReadText( p, &value, false, end, false, encoding ); - } - else if ( *p == DOUBLE_QUOTE ) - { - ++p; - end = "\""; // double quote in string - p = ReadText( p, &value, false, end, false, encoding ); - } - else - { - // All attribute values should be in single or double quotes. - // But this is such a common error that the parser will try - // its best, even without them. - value = ""; - while ( p && *p // existence - && !IsWhiteSpace( *p ) // whitespace - && *p != '/' && *p != '>' ) // tag end - { - if ( *p == SINGLE_QUOTE || *p == DOUBLE_QUOTE ) { - // [ 1451649 ] Attribute values with trailing quotes not handled correctly - // We did not have an opening quote but seem to have a - // closing one. Give up and throw an error. - if ( document ) document->SetError( TIXML_ERROR_READING_ATTRIBUTES, p, data, encoding ); - return 0; - } - value += *p; - ++p; - } - } - return p; -} - -#ifdef TIXML_USE_STL -void TiXmlText::StreamIn( std::istream * in, TIXML_STRING * tag ) -{ - while ( in->good() ) - { - int c = in->peek(); - if ( !cdata && (c == '<' ) ) - { - return; - } - if ( c <= 0 ) - { - TiXmlDocument* document = GetDocument(); - if ( document ) - document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); - return; - } - - (*tag) += (char) c; - in->get(); // "commits" the peek made above - - if ( cdata && c == '>' && tag->size() >= 3 ) { - size_t len = tag->size(); - if ( (*tag)[len-2] == ']' && (*tag)[len-3] == ']' ) { - // terminator of cdata. - return; - } - } - } -} -#endif - -const char* TiXmlText::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ) -{ - value = ""; - TiXmlDocument* document = GetDocument(); - - if ( data ) - { - data->Stamp( p, encoding ); - location = data->Cursor(); - } - - const char* const startTag = ""; - - if ( cdata || StringEqual( p, startTag, false, encoding ) ) - { - cdata = true; - - if ( !StringEqual( p, startTag, false, encoding ) ) - { - if ( document ) - document->SetError( TIXML_ERROR_PARSING_CDATA, p, data, encoding ); - return 0; - } - p += strlen( startTag ); - - // Keep all the white space, ignore the encoding, etc. - while ( p && *p - && !StringEqual( p, endTag, false, encoding ) - ) - { - value += *p; - ++p; - } - - TIXML_STRING dummy; - p = ReadText( p, &dummy, false, endTag, false, encoding ); - return p; - } - else - { - bool ignoreWhite = true; - - const char* end = "<"; - p = ReadText( p, &value, ignoreWhite, end, false, encoding ); - if ( p && *p ) - return p-1; // don't truncate the '<' - return 0; - } -} - -#ifdef TIXML_USE_STL -void TiXmlDeclaration::StreamIn( std::istream * in, TIXML_STRING * tag ) -{ - while ( in->good() ) - { - int c = in->get(); - if ( c <= 0 ) - { - TiXmlDocument* document = GetDocument(); - if ( document ) - document->SetError( TIXML_ERROR_EMBEDDED_NULL, 0, 0, TIXML_ENCODING_UNKNOWN ); - return; - } - (*tag) += (char) c; - - if ( c == '>' ) - { - // All is well. - return; - } - } -} -#endif - -const char* TiXmlDeclaration::Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding _encoding ) -{ - p = SkipWhiteSpace( p, _encoding ); - // Find the beginning, find the end, and look for - // the stuff in-between. - TiXmlDocument* document = GetDocument(); - if ( !p || !*p || !StringEqual( p, "SetError( TIXML_ERROR_PARSING_DECLARATION, 0, 0, _encoding ); - return 0; - } - if ( data ) - { - data->Stamp( p, _encoding ); - location = data->Cursor(); - } - p += 5; - - version = ""; - encoding = ""; - standalone = ""; - - while ( p && *p ) - { - if ( *p == '>' ) - { - ++p; - return p; - } - - p = SkipWhiteSpace( p, _encoding ); - if ( StringEqual( p, "version", true, _encoding ) ) - { - TiXmlAttribute attrib; - p = attrib.Parse( p, data, _encoding ); - version = attrib.Value(); - } - else if ( StringEqual( p, "encoding", true, _encoding ) ) - { - TiXmlAttribute attrib; - p = attrib.Parse( p, data, _encoding ); - encoding = attrib.Value(); - } - else if ( StringEqual( p, "standalone", true, _encoding ) ) - { - TiXmlAttribute attrib; - p = attrib.Parse( p, data, _encoding ); - standalone = attrib.Value(); - } - else - { - // Read over whatever it is. - while( p && *p && *p != '>' && !IsWhiteSpace( *p ) ) - ++p; - } - } - return 0; -} - -bool TiXmlText::Blank() const -{ - for ( unsigned i=0; i