├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── config.xml └── savedArmStates.csv ├── deploy ├── aarch64 │ └── z1_udp_service └── x86_64 │ └── z1_udp_service ├── include ├── FSM │ ├── BaseState.h │ ├── FSMState.h │ ├── FiniteStateMachine.h │ ├── State_BackToStart.h │ ├── State_Calibration.h │ ├── State_Cartesian.h │ ├── State_JointSpace.h │ ├── State_LowCmd.h │ ├── State_MoveC.h │ ├── State_MoveJ.h │ ├── State_MoveL.h │ ├── State_Passive.h │ ├── State_SaveState.h │ ├── State_Teach.h │ ├── State_TeachRepeat.h │ ├── State_ToState.h │ └── State_Trajectory.h ├── common │ ├── enumClass.h │ ├── math │ │ ├── Filter.h │ │ ├── mathTools.h │ │ ├── mathTypes.h │ │ └── robotics.h │ └── utilities │ │ ├── CSVTool.h │ │ ├── loop.h │ │ ├── timer.h │ │ └── typeTrans.h ├── control │ ├── CtrlComponents.h │ ├── armSDK.h │ ├── cmdPanel.h │ └── keyboard.h ├── interface │ ├── IOInterface.h │ └── IOUDP.h ├── message │ ├── LowlevelCmd.h │ ├── LowlevelState.h │ ├── MotorCmd.h │ ├── MotorState.h │ ├── arm_common.h │ └── udp.h ├── model │ ├── ArmModel.h │ └── unitree_gripper.h ├── thirdparty │ ├── quadProgpp │ │ ├── Array.hh │ │ └── QuadProg++.hh │ └── tinyxml │ │ ├── tinystr.h │ │ └── tinyxml.h └── trajectory │ ├── EndCircleTraj.h │ ├── EndHomoTraj.h │ ├── EndLineTraj.h │ ├── JointSpaceTraj.h │ ├── SCurve.h │ ├── StopForTime.h │ ├── Trajectory.h │ └── TrajectoryManager.h ├── lib ├── libZ1_aarch64.so └── libZ1_x86_64.so ├── main.cpp ├── sim ├── CMakeLists.txt ├── IOROS.cpp ├── IOROS.h ├── package.xml └── sim_ctrl.cpp └── unitreeArmTools.py /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | build 3 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | project(z1_controller LANGUAGES CXX) 3 | set(PACKAGE_NAME z1_controller) 4 | set(CMAKE_CXX_FLAGS "$ENV{CXXFLAGS} -O3 -std=c++14 -pthread") 5 | 6 | find_package(Boost REQUIRED) 7 | find_package(Eigen3 REQUIRED) 8 | include_directories( 9 | ${Boost_INCLUDE_DIRS} 10 | ${Eigen3_INCLUDE_DIRS} 11 | include 12 | ) 13 | 14 | link_directories(lib) 15 | # ----------------------add executable---------------------- 16 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}) 17 | add_executable(z1_ctrl main.cpp) 18 | target_link_libraries(z1_ctrl libZ1_${CMAKE_HOST_SYSTEM_PROCESSOR}.so) 19 | 20 | find_package(catkin) 21 | if(${catkin_FOUND}) 22 | add_subdirectory(sim) 23 | endif() -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2016-2022 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics") 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Documentation: 2 | 3 | [unitree-z1-docs-en](https://support.unitree.com/home/en/Z1_developer/z1) 4 | 5 | [unitree-z1-docs-cn](https://support.unitree.com/home/zh/Z1_developer/z1) 6 | -------------------------------------------------------------------------------- /config/config.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 192.168.123.110 4 | 8881 5 | 6 | 7 | N 8 | 10.0 9 | 0.0 10 | 11 | 12 | 10.0 13 | 14 | -------------------------------------------------------------------------------- /config/savedArmStates.csv: -------------------------------------------------------------------------------- 1 | forward, 0.0, 1.5, -1.0, -0.54, 0.0, 0.0, 2 | startFlat, 0.0, 0.0, -0.005, -0.074, 0.0, 0.0, 3 | show_left, -1.0, 0.9, -1., -0.3, 0.0, 0.0, 4 | show_mid, 0.0, 0.9, -1.2, 0.25, 0.0, 0.0, 5 | show_right, 1.0, 0.9, -1., -0.3, 0.0, 0.0, -------------------------------------------------------------------------------- /deploy/aarch64/z1_udp_service: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/z1_controller/b6fd77090b3ae9d4d6b7c7ad2a93373a62d70a14/deploy/aarch64/z1_udp_service -------------------------------------------------------------------------------- /deploy/x86_64/z1_udp_service: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/z1_controller/b6fd77090b3ae9d4d6b7c7ad2a93373a62d70a14/deploy/x86_64/z1_udp_service -------------------------------------------------------------------------------- /include/FSM/BaseState.h: -------------------------------------------------------------------------------- 1 | #ifndef BASESTATE_H 2 | #define BASESTATE_H 3 | 4 | #include 5 | #include "common/enumClass.h" 6 | 7 | class BaseState{ 8 | public: 9 | BaseState(int stateNameEnum, std::string stateNameString) 10 | : _stateNameEnum(stateNameEnum), _stateNameString(stateNameString){} 11 | virtual ~BaseState(){}; 12 | 13 | virtual void enter() = 0; 14 | virtual void run() = 0; 15 | virtual void exit() = 0; 16 | virtual int checkChange(int cmd) = 0; 17 | 18 | bool isState(int stateEnum){ 19 | if(_stateNameEnum == stateEnum){ 20 | return true; 21 | }else{ 22 | return false; 23 | } 24 | } 25 | std::string getStateName(){return _stateNameString;} 26 | int getStateNameEnum(){return _stateNameEnum;}; 27 | protected: 28 | int _stateNameEnum; 29 | std::string _stateNameString; 30 | }; 31 | 32 | #endif -------------------------------------------------------------------------------- /include/FSM/FSMState.h: -------------------------------------------------------------------------------- 1 | #ifndef FSMSTATE_H 2 | #define FSMSTATE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "control/CtrlComponents.h" 8 | #include "common/math/mathTools.h" 9 | #include "common/utilities/timer.h" 10 | #include "FSM/BaseState.h" 11 | #include "model/unitree_gripper.h" 12 | 13 | class FSMState : public BaseState{ 14 | public: 15 | FSMState(CtrlComponents *ctrlComp, ArmFSMStateName stateName, std::string stateNameString); 16 | virtual ~FSMState(){} 17 | 18 | virtual void enter() = 0; 19 | virtual void run() = 0; 20 | virtual void exit() = 0; 21 | virtual int checkChange(int cmd) {return (int)ArmFSMStateName::INVALID;} 22 | bool _collisionTest(); 23 | 24 | protected: 25 | void _armCtrl(); 26 | void _recordData(); 27 | Vec6 _postureToVec6(Posture posture); 28 | void _tauFriction(); 29 | void _zero_position_joint4_protection(); 30 | 31 | LowlevelCmd *_lowCmd; 32 | LowlevelState *_lowState; 33 | IOInterface *_ioInter; 34 | ArmModel *_armModel; 35 | std::shared_ptr _gripper; 36 | 37 | Vec6 _qPast, _qdPast, _q, _qd, _qdd, _tauForward; 38 | double _gripperPos, _gripperW, _gripperTau; 39 | 40 | CtrlComponents *_ctrlComp; 41 | Vec6 _g, _tauCmd, _tauFric; 42 | 43 | private: 44 | 45 | uint _collisionCnt; 46 | 47 | Vec6 _mLinearFriction; 48 | Vec6 _mCoulombFriction; 49 | 50 | }; 51 | 52 | #endif // FSMSTATE_H 53 | -------------------------------------------------------------------------------- /include/FSM/FiniteStateMachine.h: -------------------------------------------------------------------------------- 1 | #ifndef FSM_H 2 | #define FSM_H 3 | 4 | #include 5 | #include "FSM/FSMState.h" 6 | #include "common/utilities/loop.h" 7 | #include "control/CtrlComponents.h" 8 | 9 | class FiniteStateMachine{ 10 | public: 11 | FiniteStateMachine(std::vector states, CtrlComponents *ctrlComp); 12 | virtual ~FiniteStateMachine(); 13 | 14 | private: 15 | void _run(); 16 | std::vector _states; 17 | 18 | FSMMode _mode; 19 | bool _running; 20 | FSMState* _currentState; 21 | FSMState* _nextState; 22 | int _nextStateEnum; 23 | 24 | CtrlComponents *_ctrlComp; 25 | LoopFunc *_runThread; 26 | }; 27 | 28 | #endif // FSM_H -------------------------------------------------------------------------------- /include/FSM/State_BackToStart.h: -------------------------------------------------------------------------------- 1 | #ifndef STATE_BACKTOSTART_H 2 | #define STATE_BACKTOSTART_H 3 | 4 | 5 | #include "FSM/FSMState.h" 6 | #include "trajectory/JointSpaceTraj.h" 7 | 8 | class State_BackToStart : public FSMState{ 9 | public: 10 | State_BackToStart(CtrlComponents *ctrlComp); 11 | ~State_BackToStart(); 12 | void enter(); 13 | void run(); 14 | void exit(); 15 | int checkChange(int cmd); 16 | private: 17 | bool _reach, _pastReach; 18 | JointSpaceTraj *_jointTraj; 19 | Vec6 _pos_startFlat; 20 | }; 21 | 22 | #endif // STATE_BACKTOSTART_H -------------------------------------------------------------------------------- /include/FSM/State_Calibration.h: -------------------------------------------------------------------------------- 1 | #ifndef STATE_CALIBRATION_H 2 | #define STATE_CALIBRATION_H 3 | 4 | #include "FSM/FSMState.h" 5 | 6 | class State_Calibration : public FSMState{ 7 | public: 8 | State_Calibration(CtrlComponents *ctrlComp); 9 | ~State_Calibration(){} 10 | void enter(); 11 | void run(){}; 12 | void exit(){}; 13 | int checkChange(int cmd); 14 | private: 15 | 16 | }; 17 | 18 | #endif // STATE_CALIBRATION_H -------------------------------------------------------------------------------- /include/FSM/State_Cartesian.h: -------------------------------------------------------------------------------- 1 | #ifndef CARTESIAN_H 2 | #define CARTESIAN_H 3 | 4 | #include "FSM/FSMState.h" 5 | 6 | class State_Cartesian : public FSMState{ 7 | public: 8 | State_Cartesian(CtrlComponents *ctrlComp); 9 | ~State_Cartesian(){} 10 | void enter(); 11 | void run(); 12 | void exit(); 13 | int checkChange(int cmd); 14 | private: 15 | double _oriSpeed = 0.3;// control by keyboard 16 | double _posSpeed = 0.2; 17 | double oriSpeedLimit = 0.5;// limits in SDK 18 | double posSpeedLimit = 0.5; 19 | VecX _changeDirectionsf; 20 | 21 | Vec6 _twist; 22 | HomoMat _endHomoGoal, _endHomoPast, _endHomoDelta; 23 | Vec6 _endPostureGoal, _endPosturePast, _endPostureDelta; 24 | }; 25 | 26 | #endif // CARTESIAN_H -------------------------------------------------------------------------------- /include/FSM/State_JointSpace.h: -------------------------------------------------------------------------------- 1 | #ifndef JOINTSPACE_H 2 | #define JOINTSPACE_H 3 | 4 | #include "FSM/FSMState.h" 5 | 6 | class State_JointSpace : public FSMState{ 7 | public: 8 | State_JointSpace(CtrlComponents *ctrlComp); 9 | ~State_JointSpace(){} 10 | void enter(); 11 | void run(); 12 | void exit(); 13 | int checkChange(int cmd); 14 | private: 15 | std::vector jointSpeedMax; 16 | }; 17 | 18 | #endif // JOINTSPACE_H -------------------------------------------------------------------------------- /include/FSM/State_LowCmd.h: -------------------------------------------------------------------------------- 1 | #ifndef LOWCMD_H 2 | #define LOWCMD_H 3 | 4 | #include "FSMState.h" 5 | 6 | class State_LowCmd : public FSMState{ 7 | public: 8 | State_LowCmd(CtrlComponents *ctrlComp); 9 | void enter(); 10 | void run(); 11 | void exit(); 12 | int checkChange(int cmd); 13 | private: 14 | std::vector _kp; 15 | std::vector _kw; 16 | }; 17 | 18 | #endif // LOWCMD_H -------------------------------------------------------------------------------- /include/FSM/State_MoveC.h: -------------------------------------------------------------------------------- 1 | #ifndef MOVEC_H 2 | #define MOVEC_H 3 | 4 | #include "FSM/FSMState.h" 5 | #include "trajectory/EndCircleTraj.h" 6 | 7 | class State_MoveC : public FSMState{ 8 | public: 9 | State_MoveC(CtrlComponents *ctrlComp); 10 | ~State_MoveC(); 11 | void enter(); 12 | void run(); 13 | void exit(); 14 | int checkChange(int cmd); 15 | private: 16 | double _speed; 17 | std::vector _postures; 18 | EndCircleTraj *_circleTraj; 19 | bool _timeReached, _taskReached, _pastTaskReached, _finalReached; 20 | uint _taskCnt; 21 | }; 22 | 23 | #endif // CARTESIAN_H -------------------------------------------------------------------------------- /include/FSM/State_MoveJ.h: -------------------------------------------------------------------------------- 1 | #ifndef MOVEJ_H 2 | #define MOVEJ_H 3 | 4 | #include "FSM/FSMState.h" 5 | #include "trajectory/JointSpaceTraj.h" 6 | 7 | class State_MoveJ : public FSMState{ 8 | public: 9 | State_MoveJ(CtrlComponents *ctrlComp); 10 | ~State_MoveJ(); 11 | void enter(); 12 | void run(); 13 | void exit(); 14 | int checkChange(int cmd); 15 | private: 16 | double _speed; 17 | JointSpaceTraj *_jointTraj; 18 | bool _reached, _pastReached, _finalReached; 19 | std::vector _qCmd; 20 | }; 21 | 22 | #endif // CARTESIAN_H -------------------------------------------------------------------------------- /include/FSM/State_MoveL.h: -------------------------------------------------------------------------------- 1 | #ifndef MOVEL_H 2 | #define MOVEL_H 3 | 4 | #include "FSM/FSMState.h" 5 | #include "trajectory/EndLineTraj.h" 6 | 7 | class State_MoveL : public FSMState{ 8 | public: 9 | State_MoveL(CtrlComponents *ctrlComp); 10 | ~State_MoveL(); 11 | void enter(); 12 | void run(); 13 | void exit(); 14 | int checkChange(int cmd); 15 | private: 16 | double _speed; 17 | std::vector _postures; 18 | EndLineTraj *_lineTraj; 19 | bool _timeReached, _taskReached, _pastTaskReached, _finalReached; 20 | uint _taskCnt; 21 | }; 22 | #endif // CARTESIAN_H -------------------------------------------------------------------------------- /include/FSM/State_Passive.h: -------------------------------------------------------------------------------- 1 | #ifndef PASSIVE_H 2 | #define PASSIVE_H 3 | 4 | #include "FSM/FSMState.h" 5 | 6 | class State_Passive : public FSMState{ 7 | public: 8 | State_Passive(CtrlComponents *ctrlComp); 9 | void enter(); 10 | void run(); 11 | void exit(); 12 | int checkChange(int cmd); 13 | }; 14 | 15 | #endif // PASSIVE_H -------------------------------------------------------------------------------- /include/FSM/State_SaveState.h: -------------------------------------------------------------------------------- 1 | #ifndef SAVESTATE_H 2 | #define SAVESTATE_H 3 | 4 | #include "FSMState.h" 5 | #include "common/utilities/CSVTool.h" 6 | 7 | class State_SaveState : public FSMState{ 8 | public: 9 | State_SaveState(CtrlComponents *ctrlComp); 10 | ~State_SaveState(); 11 | void enter(); 12 | void run(); 13 | void exit(); 14 | int checkChange(int cmd); 15 | private: 16 | }; 17 | 18 | #endif // SAVESTATE_H -------------------------------------------------------------------------------- /include/FSM/State_Teach.h: -------------------------------------------------------------------------------- 1 | #ifndef STATETEACH_H 2 | #define STATETEACH_H 3 | 4 | #include "FSM/FSMState.h" 5 | 6 | class State_Teach : public FSMState{ 7 | public: 8 | State_Teach(CtrlComponents *ctrlComp); 9 | ~State_Teach(); 10 | void enter(); 11 | void run(); 12 | void exit(); 13 | int checkChange(int cmd); 14 | private: 15 | CSVTool *_trajCSV; 16 | size_t _stateID = 0; 17 | 18 | Vec6 _KdDiag; 19 | Vec6 _kpForStable; 20 | Mat6 _Kd; 21 | double _errorBias; 22 | }; 23 | 24 | #endif // STATETEACH_H -------------------------------------------------------------------------------- /include/FSM/State_TeachRepeat.h: -------------------------------------------------------------------------------- 1 | #ifndef STATE_TEACHREPEAT_H 2 | #define STATE_TEACHREPEAT_H 3 | 4 | #include "FSM/FSMState.h" 5 | #include "trajectory/TrajectoryManager.h" 6 | 7 | class State_TeachRepeat : public FSMState{ 8 | public: 9 | State_TeachRepeat(CtrlComponents *ctrlComp); 10 | ~State_TeachRepeat(); 11 | void enter(); 12 | void run(); 13 | void exit(); 14 | int checkChange(int cmd); 15 | private: 16 | bool _setCorrectly; 17 | JointSpaceTraj *_toStartTraj; 18 | bool _reachedStart = false; 19 | bool _finishedRepeat = false; 20 | size_t _index = 0; 21 | Vec6 _trajStartQ, _trajStartQd; 22 | double _trajStartGripperQ, _trajStartGripperQd; 23 | 24 | CSVTool *_csvFile; 25 | }; 26 | 27 | #endif // STATE_TEACHREPEAT_H -------------------------------------------------------------------------------- /include/FSM/State_ToState.h: -------------------------------------------------------------------------------- 1 | #ifndef TOSTATE_H 2 | #define TOSTATE_H 3 | 4 | #include "FSM/FSMState.h" 5 | #include "trajectory/JointSpaceTraj.h" 6 | 7 | class State_ToState : public FSMState{ 8 | public: 9 | State_ToState(CtrlComponents *ctrlComp); 10 | ~State_ToState(); 11 | void enter(); 12 | void run(); 13 | void exit(); 14 | int checkChange(int cmd); 15 | private: 16 | bool _setCorrectly; 17 | double _costTime; 18 | HomoMat _goalHomo; 19 | JointSpaceTraj *_jointTraj; 20 | bool _reach, _pastReach; 21 | std::string _goalName; 22 | }; 23 | 24 | #endif // TOSTATE_H -------------------------------------------------------------------------------- /include/FSM/State_Trajectory.h: -------------------------------------------------------------------------------- 1 | #ifndef CARTESIANPATH_H 2 | #define CARTESIANPATH_H 3 | 4 | #include "FSM/FSMState.h" 5 | #include "trajectory/TrajectoryManager.h" 6 | 7 | class State_Trajectory : public FSMState{ 8 | public: 9 | State_Trajectory(CtrlComponents *ctrlComp, 10 | ArmFSMStateName stateEnum = ArmFSMStateName::TRAJECTORY, 11 | std::string stateString = "trajectory"); 12 | ~State_Trajectory(); 13 | void enter(); 14 | void run(); 15 | void exit(); 16 | int checkChange(int cmd); 17 | protected: 18 | void _setTraj(); 19 | 20 | TrajectoryManager *_traj; 21 | HomoMat _goalHomo; 22 | Vec6 _goalTwist; 23 | double speedTemp; 24 | 25 | JointSpaceTraj *_toStartTraj; 26 | bool _reachedStart = false; 27 | bool _finishedTraj = false; 28 | std::vector _jointTraj; 29 | std::vector _circleTraj; 30 | std::vector _stopTraj; 31 | std::vector _lineTraj; 32 | 33 | static std::vector _trajCmd; 34 | Vec6 _posture[2]; 35 | }; 36 | 37 | #endif // CARTESIANPATH_H -------------------------------------------------------------------------------- /include/common/enumClass.h: -------------------------------------------------------------------------------- 1 | #ifndef ENUMCLASS_H 2 | #define ENUMCLASS_H 3 | 4 | enum class FSMMode{ 5 | NORMAL, 6 | CHANGE 7 | }; 8 | 9 | enum class ArmFSMStateName{ 10 | INVALID, 11 | PASSIVE, 12 | JOINTCTRL, 13 | CARTESIAN, 14 | MOVEJ, 15 | MOVEL, 16 | MOVEC, 17 | TRAJECTORY, 18 | TOSTATE, 19 | SAVESTATE, 20 | TEACH, 21 | TEACHREPEAT, 22 | CALIBRATION, 23 | SETTRAJ,//no longer used 24 | BACKTOSTART, 25 | NEXT, 26 | LOWCMD 27 | }; 28 | 29 | enum class JointMotorType{ 30 | SINGLE_MOTOR, 31 | DOUBLE_MOTOR 32 | }; 33 | 34 | enum class Control{ 35 | KEYBOARD, 36 | SDK 37 | }; 38 | 39 | #endif // ENUMCLASS_H -------------------------------------------------------------------------------- /include/common/math/Filter.h: -------------------------------------------------------------------------------- 1 | #ifndef FILTER 2 | #define FILTER 3 | 4 | #include 5 | #include 6 | 7 | /* 8 | Low pass filter 9 | */ 10 | class LPFilter{ 11 | public: 12 | LPFilter(double samplePeriod, double cutFrequency, size_t valueCount); 13 | ~LPFilter(){}; 14 | void clear(); 15 | void addValue(double &newValue); 16 | void addValue(std::vector &vec); 17 | 18 | template 19 | void addValue(Eigen::MatrixBase &eigenVec); 20 | 21 | // double getValue(); 22 | private: 23 | size_t _valueCount; 24 | double _weight; 25 | std::vector _pastValue; 26 | bool _start; 27 | }; 28 | 29 | #endif // FILTER -------------------------------------------------------------------------------- /include/common/math/mathTools.h: -------------------------------------------------------------------------------- 1 | #ifndef MATHTOOLS_H 2 | #define MATHTOOLS_H 3 | 4 | #include 5 | #include 6 | #include "common/math/mathTypes.h" 7 | 8 | template 9 | T max(T value){ 10 | return value; 11 | } 12 | 13 | template 14 | inline T max(const T val0, const Args... restVal){ 15 | return val0 > (max(restVal...)) ? val0 : max(restVal...); 16 | } 17 | 18 | template 19 | T min(T value){ 20 | return value; 21 | } 22 | 23 | template 24 | inline T min(const T val0, const Args... restVal){ 25 | return val0 > (min(restVal...)) ? val0 : min(restVal...); 26 | } 27 | 28 | inline double clamp(const double& x, const double& low_value, const double& high_value) { 29 | return (x > low_value) ? (x0?1:-1 ); 34 | } 35 | 36 | 37 | enum class TurnDirection{ 38 | NOMATTER, 39 | POSITIVE, 40 | NEGATIVE 41 | }; 42 | 43 | /* first - second */ 44 | inline double angleError(double first, double second, TurnDirection direction = TurnDirection::NOMATTER){ 45 | double firstMod = fmod(first, 2.0*M_PI); 46 | double secondMod = fmod(second, 2.0*M_PI); 47 | 48 | if(direction == TurnDirection::POSITIVE){ 49 | if(firstMod - secondMod < 0.0){ 50 | return 2*M_PI + firstMod - secondMod; 51 | }else{ 52 | return firstMod - secondMod; 53 | } 54 | } 55 | else if(direction == TurnDirection::NEGATIVE){ 56 | if(firstMod - secondMod > 0.0){ 57 | return -2*M_PI + firstMod - secondMod; 58 | }else{ 59 | return firstMod - secondMod; 60 | } 61 | }else{//no matter 62 | if(fabs(firstMod - secondMod) > fabs(secondMod - firstMod)){ 63 | return secondMod - firstMod; 64 | }else{ 65 | return firstMod - secondMod; 66 | } 67 | } 68 | } 69 | 70 | /* firstVec - secondVec */ 71 | inline VecX angleError(VecX firstVec, VecX secondVec, TurnDirection directionMatter = TurnDirection::NOMATTER){ 72 | if(firstVec.rows() != secondVec.rows()){ 73 | std::cout << "[ERROR] angleError, the sizes of firstVec and secondVec are different!" << std::endl; 74 | } 75 | 76 | VecX result = firstVec; 77 | for(int i(0); itolerance){ 92 | return false; 93 | } 94 | } 95 | return true; 96 | } 97 | 98 | inline bool inInterval(double value, double limValue1, double limValue2, bool canEqual1 = false, bool canEqual2 = false){ 99 | double lowLim, highLim; 100 | bool lowEqual, highEqual; 101 | if(limValue1 >= limValue2){ 102 | highLim = limValue1; 103 | highEqual = canEqual1; 104 | lowLim = limValue2; 105 | lowEqual = canEqual2; 106 | }else{ 107 | lowLim = limValue1; 108 | lowEqual = canEqual1; 109 | highLim = limValue2; 110 | highEqual = canEqual2; 111 | } 112 | 113 | if((value > highLim) || (value < lowLim)){ 114 | return false; 115 | } 116 | if((value == highLim) && !highEqual){ 117 | return false; 118 | } 119 | if((value == lowLim) && !lowEqual){ 120 | return false; 121 | } 122 | 123 | return true; 124 | } 125 | 126 | inline double saturation(const double a, double limValue1, double limValue2){ 127 | double lowLim, highLim; 128 | if(limValue1 >= limValue2){ 129 | lowLim = limValue2; 130 | highLim= limValue1; 131 | }else{ 132 | lowLim = limValue1; 133 | highLim= limValue2; 134 | } 135 | 136 | if(a < lowLim){ 137 | return lowLim; 138 | }else if(a > highLim){ 139 | return highLim; 140 | }else{ 141 | return a; 142 | } 143 | } 144 | 145 | inline double saturation(const double a, Vec2 limits){ 146 | return saturation(a, limits(0), limits(1)); 147 | } 148 | 149 | template 150 | inline T0 killZeroOffset(T0 a, const T1 limit){ 151 | if((a > -limit) && (a < limit)){ 152 | a = 0; 153 | } 154 | return a; 155 | } 156 | 157 | template 158 | inline T1 invNormalize(const T0 value, const T1 min, const T2 max, const double minLim = -1, const double maxLim = 1){ 159 | return (value-minLim)*(max-min)/(maxLim-minLim) + min; 160 | } 161 | 162 | // x: [0, 1], windowRatio: (0, 0.5) 163 | template 164 | inline T windowFunc(const T x, const T windowRatio, const T xRange=1.0, const T yRange=1.0){ 165 | if((x < 0)||(x > xRange)){ 166 | std::cout << "[ERROR][windowFunc] The x=" << x << ", which should between [0, xRange]" << std::endl; 167 | } 168 | if((windowRatio <= 0)||(windowRatio >= 0.5)){ 169 | std::cout << "[ERROR][windowFunc] The windowRatio=" << windowRatio << ", which should between [0, 0.5]" << std::endl; 170 | } 171 | 172 | if(x/xRange < windowRatio){ 173 | return x * yRange / (xRange * windowRatio); 174 | }else if(x/xRange > 1 - windowRatio){ 175 | return yRange * (xRange - x)/(xRange * windowRatio); 176 | }else{ 177 | return yRange; 178 | } 179 | } 180 | 181 | template 182 | inline void updateAverage(T1 &exp, T2 newValue, double n){ 183 | if(exp.rows()!=newValue.rows()){ 184 | std::cout << "The size of updateAverage is error" << std::endl; 185 | exit(-1); 186 | } 187 | if(fabs(n - 1) < 0.001){ 188 | exp = newValue; 189 | }else{ 190 | exp = exp + (newValue - exp)/n; 191 | } 192 | } 193 | 194 | template 195 | inline void updateCovariance(T1 &cov, T2 expPast, T3 newValue, double n){ 196 | if( (cov.rows()!=cov.cols()) || (cov.rows() != expPast.rows()) || (expPast.rows()!=newValue.rows())){ 197 | std::cout << "The size of updateCovariance is error" << std::endl; 198 | exit(-1); 199 | } 200 | if(fabs(n - 1) < 0.1){ 201 | cov.setZero(); 202 | }else{ 203 | cov = cov*(n-1)/n + (newValue-expPast)*(newValue-expPast).transpose()*(n-1)/(n*n); 204 | } 205 | } 206 | 207 | template 208 | inline void updateAvgCov(T1 &cov, T2 &exp, T3 newValue, double n){ 209 | // The order matters!!! covariance first!!! 210 | updateCovariance(cov, exp, newValue, n); 211 | updateAverage(exp, newValue, n); 212 | } 213 | 214 | // Calculate average value and covariance 215 | class AvgCov{ 216 | public: 217 | AvgCov(unsigned int size, std::string name, bool avgOnly=false, unsigned int showPeriod=1000, unsigned int waitCount=5000, double zoomFactor=10000) 218 | :_size(size), _showPeriod(showPeriod), _waitCount(waitCount), _zoomFactor(zoomFactor), _valueName(name), _avgOnly(avgOnly) { 219 | _exp.resize(size); 220 | _cov.resize(size, size); 221 | _defaultWeight.resize(size, size); 222 | _defaultWeight.setIdentity(); 223 | _measureCount = 0; 224 | } 225 | void measure(VecX newValue){ 226 | ++_measureCount; 227 | 228 | if(_measureCount > _waitCount){ 229 | updateAvgCov(_cov, _exp, newValue, _measureCount-_waitCount); 230 | if(_measureCount % _showPeriod == 0){ 231 | // if(_measureCount < _waitCount + 5){ 232 | std::cout << "******" << _valueName << " measured count: " << _measureCount-_waitCount << "******" << std::endl; 233 | // std::cout << _zoomFactor << " Times newValue of " << _valueName << std::endl << (_zoomFactor*newValue).transpose() << std::endl; 234 | std::cout << _zoomFactor << " Times Average of " << _valueName << std::endl << (_zoomFactor*_exp).transpose() << std::endl; 235 | if(!_avgOnly){ 236 | std::cout << _zoomFactor << " Times Covariance of " << _valueName << std::endl << _zoomFactor*_cov << std::endl; 237 | } 238 | } 239 | } 240 | } 241 | private: 242 | VecX _exp; 243 | MatX _cov; 244 | MatX _defaultWeight; 245 | bool _avgOnly; 246 | unsigned int _size; 247 | unsigned int _measureCount; 248 | unsigned int _showPeriod; 249 | unsigned int _waitCount; 250 | double _zoomFactor; 251 | std::string _valueName; 252 | }; 253 | #endif -------------------------------------------------------------------------------- /include/common/math/mathTypes.h: -------------------------------------------------------------------------------- 1 | #ifndef MATHTYPES_H 2 | #define MATHTYPES_H 3 | 4 | #include 5 | #include 6 | 7 | /************************/ 8 | /******** Vector ********/ 9 | /************************/ 10 | // 2x1 Vector 11 | using Vec2 = typename Eigen::Matrix; 12 | 13 | // 3x1 Vector 14 | using Vec3 = typename Eigen::Matrix; 15 | 16 | // 4x1 Vector 17 | using Vec4 = typename Eigen::Matrix; 18 | 19 | // 6x1 Vector 20 | using Vec6 = typename Eigen::Matrix; 21 | 22 | // Quaternion 23 | using Quat = typename Eigen::Matrix; 24 | 25 | // 4x1 Integer Vector 26 | using VecInt4 = typename Eigen::Matrix; 27 | 28 | // 12x1 Vector 29 | using Vec12 = typename Eigen::Matrix; 30 | 31 | // 18x1 Vector 32 | using Vec18 = typename Eigen::Matrix; 33 | 34 | // Dynamic Length Vector 35 | using VecX = typename Eigen::Matrix; 36 | 37 | /************************/ 38 | /******** Matrix ********/ 39 | /************************/ 40 | // Rotation Matrix 41 | using RotMat = typename Eigen::Matrix; 42 | 43 | // Homogenous Matrix 44 | using HomoMat = typename Eigen::Matrix; 45 | 46 | // 2x2 Matrix 47 | using Mat2 = typename Eigen::Matrix; 48 | 49 | // 3x3 Matrix 50 | using Mat3 = typename Eigen::Matrix; 51 | 52 | // 3x4 Matrix, each column is a 3x1 vector 53 | using Vec34 = typename Eigen::Matrix; 54 | 55 | // 6x6 Matrix 56 | using Mat6 = typename Eigen::Matrix; 57 | 58 | // 12x12 Matrix 59 | using Mat12 = typename Eigen::Matrix; 60 | 61 | // 3x3 Identity Matrix 62 | #define I3 Eigen::MatrixXd::Identity(3, 3) 63 | 64 | // 6x6 Identity Matrix 65 | #define I6 Eigen::MatrixXd::Identity(6, 6) 66 | 67 | // 12x12 Identity Matrix 68 | #define I12 Eigen::MatrixXd::Identity(12, 12) 69 | 70 | // 18x18 Identity Matrix 71 | #define I18 Eigen::MatrixXd::Identity(18, 18) 72 | 73 | // Dynamic Size Matrix 74 | using MatX = typename Eigen::Matrix; 75 | 76 | /************************/ 77 | /****** Functions *******/ 78 | /************************/ 79 | inline Vec34 vec12ToVec34(Vec12 vec12){ 80 | Vec34 vec34; 81 | for(int i(0); i < 4; ++i){ 82 | vec34.col(i) = vec12.segment(3*i, 3); 83 | } 84 | return vec34; 85 | } 86 | 87 | inline Vec12 vec34ToVec12(Vec34 vec34){ 88 | Vec12 vec12; 89 | for(int i(0); i < 4; ++i){ 90 | vec12.segment(3*i, 3) = vec34.col(i); 91 | } 92 | return vec12; 93 | } 94 | 95 | template 96 | inline VecX stdVecToEigenVec(T stdVec){ 97 | VecX eigenVec = Eigen::VectorXd::Map(&stdVec[0], stdVec.size()); 98 | return eigenVec; 99 | } 100 | 101 | inline std::vector EigenVectostdVec(VecX eigenVec){ 102 | std::vector stdVec; 103 | for(int i(0); i 4 | #include "common/math/mathTools.h" 5 | 6 | namespace robo { 7 | /* 8 | * Function: Find if the value is negligible enough to consider 0 9 | * Inputs: value to be checked as a double 10 | * Returns: Boolean of true-ignore or false-can't ignore 11 | */ 12 | bool NearZero(const double); 13 | 14 | Mat6 rot(const Mat3& E); 15 | 16 | Mat6 xlt(const Vec3& r); 17 | 18 | /* rotate matrix about x axis */ 19 | RotMat rotX(const double &); 20 | RotMat rx(const double &xrot); 21 | 22 | /* rotate matrix about y axis */ 23 | RotMat rotY(const double &); 24 | RotMat ry(const double &yrot); 25 | 26 | /* rotate matrix about z axis */ 27 | RotMat rotZ(const double &); 28 | RotMat rz(const double &zrot); 29 | 30 | /* row pitch yaw to rotate matrix */ 31 | RotMat rpyToRotMat(const double&, const double&, const double&); 32 | RotMat rpyToRotMat(const Vec3& rpy); 33 | 34 | Vec3 rotMatToRPY(const Mat3& ); 35 | 36 | RotMat quatToRotMat(const Quat&); 37 | 38 | /* convert homogeneous matrix to posture vector */ 39 | Vec6 homoToPosture(HomoMat); 40 | 41 | /* convert posture vector matrix to homogeneous */ 42 | HomoMat postureToHomo(Vec6); 43 | 44 | RotMat getHomoRotMat(HomoMat T); 45 | Vec3 getHomoPosition(HomoMat T); 46 | HomoMat homoMatrix(Vec3 x, Vec3 y, Vec3 p); 47 | /* 48 | * Function: Calculate the 6x6 matrix [adV] of the given 6-vector 49 | * Input: Eigen::VectorXd (6x1) 50 | * Output: Eigen::MatrixXd (6x6) 51 | * Note: Can be used to calculate the Lie bracket [V1, V2] = [adV1]V2 52 | */ 53 | Eigen::MatrixXd ad(Eigen::VectorXd); 54 | 55 | 56 | /* 57 | * Function: Returns a normalized version of the input vector 58 | * Input: Eigen::MatrixXd 59 | * Output: Eigen::MatrixXd 60 | * Note: MatrixXd is used instead of VectorXd for the case of row vectors 61 | * Requires a copy 62 | * Useful because of the MatrixXd casting 63 | */ 64 | Eigen::MatrixXd Normalize(Eigen::MatrixXd); 65 | 66 | 67 | /* 68 | * Function: Returns the skew symmetric matrix representation of an angular velocity vector 69 | * Input: Eigen::Vector3d 3x1 angular velocity vector 70 | * Returns: Eigen::MatrixXd 3x3 skew symmetric matrix 71 | */ 72 | Eigen::Matrix3d VecToso3(const Eigen::Vector3d&); 73 | 74 | 75 | /* 76 | * Function: Returns angular velocity vector represented by the skew symmetric matrix 77 | * Inputs: Eigen::MatrixXd 3x3 skew symmetric matrix 78 | * Returns: Eigen::Vector3d 3x1 angular velocity 79 | */ 80 | Eigen::Vector3d so3ToVec(const Eigen::MatrixXd&); 81 | 82 | 83 | /* 84 | * Function: Tranlates an exponential rotation into it's individual components 85 | * Inputs: Exponential rotation (rotation matrix in terms of a rotation axis 86 | * and the angle of rotation) 87 | * Returns: The axis and angle of rotation as [x, y, z, theta] 88 | */ 89 | Eigen::Vector4d AxisAng3(const Eigen::Vector3d&); 90 | 91 | 92 | /* 93 | * Function: Translates an exponential rotation into a rotation matrix 94 | * Inputs: exponenential representation of a rotation 95 | * Returns: Rotation matrix 96 | */ 97 | Eigen::Matrix3d MatrixExp3(const Eigen::Matrix3d&); 98 | 99 | 100 | /* Function: Computes the matrix logarithm of a rotation matrix 101 | * Inputs: Rotation matrix 102 | * Returns: matrix logarithm of a rotation 103 | */ 104 | Eigen::Matrix3d MatrixLog3(const Eigen::Matrix3d&); 105 | 106 | 107 | /* 108 | * Function: Combines a rotation matrix and position vector into a single 109 | * Special Euclidian Group (SE3) homogeneous transformation matrix 110 | * Inputs: Rotation Matrix (R), Position Vector (p) 111 | * Returns: Matrix of T = [ [R, p], 112 | * [0, 1] ] 113 | */ 114 | Eigen::MatrixXd RpToTrans(const Eigen::Matrix3d&, const Eigen::Vector3d&); 115 | 116 | 117 | /* 118 | * Function: Separates the rotation matrix and position vector from 119 | * the transfomation matrix representation 120 | * Inputs: Homogeneous transformation matrix 121 | * Returns: std::vector of [rotation matrix, position vector] 122 | */ 123 | std::vector TransToRp(const Eigen::MatrixXd&); 124 | 125 | 126 | /* 127 | * Function: Translates a spatial velocity vector into a transformation matrix 128 | * Inputs: Spatial velocity vector [angular velocity, linear velocity] 129 | * Returns: Transformation matrix 130 | */ 131 | Eigen::MatrixXd VecTose3(const Eigen::VectorXd&); 132 | 133 | 134 | /* Function: Translates a transformation matrix into a spatial velocity vector 135 | * Inputs: Transformation matrix 136 | * Returns: Spatial velocity vector [angular velocity, linear velocity] 137 | */ 138 | Eigen::VectorXd se3ToVec(const Eigen::MatrixXd&); 139 | 140 | 141 | /* 142 | * Function: Provides the adjoint representation of a transformation matrix 143 | * Used to change the frame of reference for spatial velocity vectors 144 | * Inputs: 4x4 Transformation matrix SE(3) 145 | * Returns: 6x6 Adjoint Representation of the matrix 146 | */ 147 | Eigen::MatrixXd Adjoint(const Eigen::MatrixXd&); 148 | 149 | 150 | /* 151 | * Function: Rotation expanded for screw axis 152 | * Inputs: se3 matrix representation of exponential coordinates (transformation matrix) 153 | * Returns: 6x6 Matrix representing the rotation 154 | */ 155 | Eigen::MatrixXd MatrixExp6(const Eigen::MatrixXd&); 156 | 157 | 158 | /* 159 | * Function: Computes the matrix logarithm of a homogeneous transformation matrix 160 | * Inputs: R: Transformation matrix in SE3 161 | * Returns: The matrix logarithm of R 162 | */ 163 | Eigen::MatrixXd MatrixLog6(const Eigen::MatrixXd&); 164 | 165 | 166 | /* 167 | * Functions: Tranforms 3D motion vector form A to B coordinates 168 | * Input: T: the cordinate transform form A to B coordiantes for a motion vector 169 | * Return : BX_A 170 | */ 171 | Mat6 CoordinateTransMotionVector(const HomoMat& T); 172 | 173 | /* 174 | * Functions: Tranforms 3D force vector form A to B coordinates 175 | * Input: T: the cordinate transform form A to B coordiantes for a force vector 176 | * Return : {BX_A}* 177 | */ 178 | Mat6 CoordinateTransForceVector(const HomoMat& T); 179 | 180 | 181 | /* 182 | * Function: Compute end effector frame (used for current spatial position calculation) 183 | * Inputs: Home configuration (position and orientation) of end-effector 184 | * The joint screw axes in the space frame when the manipulator 185 | * is at the home position 186 | * A list of joint coordinates. 187 | * Returns: Transfomation matrix representing the end-effector frame when the joints are 188 | * at the specified coordinates 189 | * Notes: FK means Forward Kinematics 190 | */ 191 | Eigen::MatrixXd FKinSpace(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::VectorXd&); 192 | 193 | /* 194 | * Function: Compute end effector frame (used for current body position calculation) 195 | * Inputs: Home configuration (position and orientation) of end-effector 196 | * The joint screw axes in the body frame when the manipulator 197 | * is at the home position 198 | * A list of joint coordinates. 199 | * Returns: Transfomation matrix representing the end-effector frame when the joints are 200 | * at the specified coordinates 201 | * Notes: FK means Forward Kinematics 202 | */ 203 | Eigen::MatrixXd FKinBody(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::VectorXd&); 204 | 205 | 206 | /* 207 | * Function: Gives the space Jacobian 208 | * Inputs: Screw axis in home position, joint configuration 209 | * Returns: 6xn Spatial Jacobian 210 | */ 211 | Eigen::MatrixXd JacobianSpace(const Eigen::MatrixXd&, const Eigen::MatrixXd&); 212 | 213 | 214 | /* 215 | * Function: Gives the body Jacobian 216 | * Inputs: Screw axis in BODY position, joint configuration 217 | * Returns: 6xn Bobdy Jacobian 218 | */ 219 | Eigen::MatrixXd JacobianBody(const Eigen::MatrixXd&, const Eigen::MatrixXd&); 220 | 221 | 222 | /* 223 | * Inverts a homogeneous transformation matrix 224 | * Inputs: A homogeneous transformation Matrix T 225 | * Returns: The inverse of T 226 | */ 227 | Eigen::MatrixXd TransInv(const Eigen::MatrixXd&); 228 | 229 | /* 230 | * Inverts a rotation matrix 231 | * Inputs: A rotation matrix R 232 | * Returns: The inverse of R 233 | */ 234 | Eigen::MatrixXd RotInv(const Eigen::MatrixXd&); 235 | 236 | /* 237 | * Takes a parametric description of a screw axis and converts it to a 238 | * normalized screw axis 239 | * Inputs: 240 | * q: A point lying on the screw axis 241 | * s: A unit vector in the direction of the screw axis 242 | * h: The pitch of the screw axis 243 | * Returns: A normalized screw axis described by the inputs 244 | */ 245 | Eigen::VectorXd ScrewToAxis(Eigen::Vector3d q, Eigen::Vector3d s, double h); 246 | 247 | 248 | /* 249 | * Function: Translates a 6-vector of exponential coordinates into screw 250 | * axis-angle form 251 | * Inputs: 252 | * expc6: A 6-vector of exponential coordinates for rigid-body motion 253 | S*theta 254 | * Returns: The corresponding normalized screw axis S; The distance theta traveled 255 | * along/about S in form [S, theta] 256 | * Note: Is it better to return std::map? 257 | */ 258 | Eigen::VectorXd AxisAng6(const Eigen::VectorXd&); 259 | 260 | 261 | /* 262 | * Function: Returns projection of one matrix into SO(3) 263 | * Inputs: 264 | * M: A matrix near SO(3) to project to SO(3) 265 | * Returns: The closest matrix R that is in SO(3) 266 | * Projects a matrix mat to the closest matrix in SO(3) using singular-value decomposition 267 | * (see http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review). 268 | * This function is only appropriate for matrices close to SO(3). 269 | */ 270 | Eigen::MatrixXd ProjectToSO3(const Eigen::MatrixXd&); 271 | 272 | 273 | /* 274 | * Function: Returns projection of one matrix into SE(3) 275 | * Inputs: 276 | * M: A 4x4 matrix near SE(3) to project to SE(3) 277 | * Returns: The closest matrix T that is in SE(3) 278 | * Projects a matrix mat to the closest matrix in SO(3) using singular-value decomposition 279 | * (see http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review). 280 | * This function is only appropriate for matrices close to SE(3). 281 | */ 282 | Eigen::MatrixXd ProjectToSE3(const Eigen::MatrixXd&); 283 | 284 | 285 | /* 286 | * Function: Returns the Frobenius norm to describe the distance of M from the SO(3) manifold 287 | * Inputs: 288 | * M: A 3x3 matrix 289 | * Outputs: 290 | * the distance from mat to the SO(3) manifold using the following 291 | * method: 292 | * If det(M) <= 0, return a large number. 293 | * If det(M) > 0, return norm(M^T*M - I). 294 | */ 295 | double DistanceToSO3(const Eigen::Matrix3d&); 296 | 297 | 298 | /* 299 | * Function: Returns the Frobenius norm to describe the distance of mat from the SE(3) manifold 300 | * Inputs: 301 | * T: A 4x4 matrix 302 | * Outputs: 303 | * the distance from T to the SE(3) manifold using the following 304 | * method: 305 | * Compute the determinant of matR, the top 3x3 submatrix of T. 306 | * If det(matR) <= 0, return a large number. 307 | * If det(matR) > 0, replace the top 3x3 submatrix of mat with matR^T*matR, 308 | * and set the first three entries of the fourth column of mat to zero. Then 309 | * return norm(T - I). 310 | */ 311 | double DistanceToSE3(const Eigen::Matrix4d&); 312 | 313 | 314 | /* 315 | * Function: Returns true if M is close to or on the manifold SO(3) 316 | * Inputs: 317 | * M: A 3x3 matrix 318 | * Outputs: 319 | * true if M is very close to or in SO(3), false otherwise 320 | */ 321 | bool TestIfSO3(const Eigen::Matrix3d&); 322 | 323 | 324 | /* 325 | * Function: Returns true if T is close to or on the manifold SE(3) 326 | * Inputs: 327 | * M: A 4x4 matrix 328 | * Outputs: 329 | * true if T is very close to or in SE(3), false otherwise 330 | */ 331 | bool TestIfSE3(const Eigen::Matrix4d&); 332 | 333 | 334 | /* 335 | * Function: Computes inverse kinematics in the body frame for an open chain robot 336 | * Inputs: 337 | * Blist: The joint screw axes in the end-effector frame when the 338 | * manipulator is at the home position, in the format of a 339 | * matrix with axes as the columns 340 | * M: The home configuration of the end-effector 341 | * T: The desired end-effector configuration Tsd 342 | * thetalist[in][out]: An initial guess and result output of joint angles that are close to 343 | * satisfying Tsd 344 | * emog: A small positive tolerance on the end-effector orientation 345 | * error. The returned joint angles must give an end-effector 346 | * orientation error less than eomg 347 | * ev: A small positive tolerance on the end-effector linear position 348 | * error. The returned joint angles must give an end-effector 349 | * position error less than ev 350 | * Outputs: 351 | * success: A logical value where TRUE means that the function found 352 | * a solution and FALSE means that it ran through the set 353 | * number of maximum iterations without finding a solution 354 | * within the tolerances eomg and ev. 355 | * thetalist[in][out]: Joint angles that achieve T within the specified tolerances, 356 | */ 357 | bool IKinBody(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, Eigen::VectorXd&, double, double); 358 | 359 | 360 | /* 361 | * Function: Computes inverse kinematics in the space frame for an open chain robot 362 | * Inputs: 363 | * Slist: The joint screw axes in the space frame when the 364 | * manipulator is at the home position, in the format of a 365 | * matrix with axes as the columns 366 | * M: The home configuration of the end-effector 367 | * T: The desired end-effector configuration Tsd 368 | * thetalist[in][out]: An initial guess and result output of joint angles that are close to 369 | * satisfying Tsd 370 | * emog: A small positive tolerance on the end-effector orientation 371 | * error. The returned joint angles must give an end-effector 372 | * orientation error less than eomg 373 | * ev: A small positive tolerance on the end-effector linear position 374 | * error. The returned joint angles must give an end-effector 375 | * position error less than ev 376 | * Outputs: 377 | * success: A logical value where TRUE means that the function found 378 | * a solution and FALSE means that it ran through the set 379 | * number of maximum iterations without finding a solution 380 | * within the tolerances eomg and ev. 381 | * thetalist[in][out]: Joint angles that achieve T within the specified tolerances, 382 | */ 383 | bool IKinSpace(const Eigen::MatrixXd&, const Eigen::MatrixXd&, const Eigen::MatrixXd&, Eigen::VectorXd&, double, double); 384 | 385 | /* 386 | * Function: This function uses forward-backward Newton-Euler iterations to solve the 387 | * equation: 388 | * taulist = Mlist(thetalist) * ddthetalist + c(thetalist, dthetalist) ... 389 | * + g(thetalist) + Jtr(thetalist) * Ftip 390 | * Inputs: 391 | * thetalist: n-vector of joint variables 392 | * dthetalist: n-vector of joint rates 393 | * ddthetalist: n-vector of joint accelerations 394 | * g: Gravity vector g 395 | * Ftip: Spatial force applied by the end-effector expressed in frame {n+1} 396 | * Mlist: List of link frames {i} relative to {i-1} at the home position 397 | * Glist: Spatial inertia matrices Gi of the links 398 | * Slist: Screw axes Si of the joints in a space frame, in the format 399 | * of a matrix with the screw axes as the columns. 400 | * 401 | * Outputs: 402 | * taulist: The n-vector of required joint forces/torques 403 | * 404 | */ 405 | Eigen::VectorXd InverseDynamics(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, 406 | const Eigen::VectorXd&, const Eigen::VectorXd&, const std::vector&, 407 | const std::vector&, const Eigen::MatrixXd&); 408 | 409 | /* 410 | * Function: This function calls InverseDynamics with Ftip = 0, dthetalist = 0, and 411 | * ddthetalist = 0. The purpose is to calculate one important term in the dynamics equation 412 | * Inputs: 413 | * thetalist: n-vector of joint variables 414 | * g: Gravity vector g 415 | * Mlist: List of link frames {i} relative to {i-1} at the home position 416 | * Glist: Spatial inertia matrices Gi of the links 417 | * Slist: Screw axes Si of the joints in a space frame, in the format 418 | * of a matrix with the screw axes as the columns. 419 | * 420 | * Outputs: 421 | * grav: The 3-vector showing the effect force of gravity to the dynamics 422 | * 423 | */ 424 | Eigen::VectorXd GravityForces(const Eigen::VectorXd&, const Eigen::VectorXd&, 425 | const std::vector&, const std::vector&, const Eigen::MatrixXd&); 426 | 427 | /* 428 | * Function: This function calls InverseDynamics n times, each time passing a 429 | * ddthetalist vector with a single element equal to one and all other 430 | * inputs set to zero. Each call of InverseDynamics generates a single 431 | * column, and these columns are assembled to create the inertia matrix. 432 | * 433 | * Inputs: 434 | * thetalist: n-vector of joint variables 435 | * Mlist: List of link frames {i} relative to {i-1} at the home position 436 | * Glist: Spatial inertia matrices Gi of the links 437 | * Slist: Screw axes Si of the joints in a space frame, in the format 438 | * of a matrix with the screw axes as the columns. 439 | * 440 | * Outputs: 441 | * M: The numerical inertia matrix M(thetalist) of an n-joint serial 442 | * chain at the given configuration thetalist. 443 | */ 444 | Eigen::MatrixXd MassMatrix(const Eigen::VectorXd&, 445 | const std::vector&, const std::vector&, const Eigen::MatrixXd&); 446 | 447 | /* 448 | * Function: This function calls InverseDynamics with g = 0, Ftip = 0, and 449 | * ddthetalist = 0. 450 | * 451 | * Inputs: 452 | * thetalist: n-vector of joint variables 453 | * dthetalist: A list of joint rates 454 | * Mlist: List of link frames {i} relative to {i-1} at the home position 455 | * Glist: Spatial inertia matrices Gi of the links 456 | * Slist: Screw axes Si of the joints in a space frame, in the format 457 | * of a matrix with the screw axes as the columns. 458 | * 459 | * Outputs: 460 | * c: The vector c(thetalist,dthetalist) of Coriolis and centripetal 461 | * terms for a given thetalist and dthetalist. 462 | */ 463 | Eigen::VectorXd VelQuadraticForces(const Eigen::VectorXd&, const Eigen::VectorXd&, 464 | const std::vector&, const std::vector&, const Eigen::MatrixXd&); 465 | 466 | /* 467 | * Function: This function calls InverseDynamics with g = 0, dthetalist = 0, and 468 | * ddthetalist = 0. 469 | * 470 | * Inputs: 471 | * thetalist: n-vector of joint variables 472 | * Ftip: Spatial force applied by the end-effector expressed in frame {n+1} 473 | * Mlist: List of link frames {i} relative to {i-1} at the home position 474 | * Glist: Spatial inertia matrices Gi of the links 475 | * Slist: Screw axes Si of the joints in a space frame, in the format 476 | * of a matrix with the screw axes as the columns. 477 | * 478 | * Outputs: 479 | * JTFtip: The joint forces and torques required only to create the 480 | * end-effector force Ftip. 481 | */ 482 | Eigen::VectorXd EndEffectorForces(const Eigen::VectorXd&, const Eigen::VectorXd&, 483 | const std::vector&, const std::vector&, const Eigen::MatrixXd&); 484 | 485 | /* 486 | * Function: This function computes ddthetalist by solving: 487 | * Mlist(thetalist) * ddthetalist = taulist - c(thetalist,dthetalist) 488 | * - g(thetalist) - Jtr(thetalist) * Ftip 489 | * Inputs: 490 | * thetalist: n-vector of joint variables 491 | * dthetalist: n-vector of joint rates 492 | * taulist: An n-vector of joint forces/torques 493 | * g: Gravity vector g 494 | * Ftip: Spatial force applied by the end-effector expressed in frame {n+1} 495 | * Mlist: List of link frames {i} relative to {i-1} at the home position 496 | * Glist: Spatial inertia matrices Gi of the links 497 | * Slist: Screw axes Si of the joints in a space frame, in the format 498 | * of a matrix with the screw axes as the columns. 499 | * 500 | * Outputs: 501 | * ddthetalist: The resulting joint accelerations 502 | * 503 | */ 504 | Eigen::VectorXd ForwardDynamics(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, 505 | const Eigen::VectorXd&, const Eigen::VectorXd&, const std::vector&, 506 | const std::vector&, const Eigen::MatrixXd&); 507 | 508 | 509 | /* 510 | * Function: Compute the joint control torques at a particular time instant 511 | * Inputs: 512 | * thetalist: n-vector of joint variables 513 | * dthetalist: n-vector of joint rates 514 | * eint: n-vector of the time-integral of joint errors 515 | * g: Gravity vector g 516 | * Mlist: List of link frames {i} relative to {i-1} at the home position 517 | * Glist: Spatial inertia matrices Gi of the links 518 | * Slist: Screw axes Si of the joints in a space frame, in the format 519 | * of a matrix with the screw axes as the columns. 520 | * thetalistd: n-vector of reference joint variables 521 | * dthetalistd: n-vector of reference joint rates 522 | * ddthetalistd: n-vector of reference joint accelerations 523 | * Kp: The feedback proportional gain (identical for each joint) 524 | * Ki: The feedback integral gain (identical for each joint) 525 | * Kd: The feedback derivative gain (identical for each joint) 526 | * 527 | * Outputs: 528 | * tau_computed: The vector of joint forces/torques computed by the feedback 529 | * linearizing controller at the current instant 530 | */ 531 | Eigen::VectorXd ComputedTorque(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, 532 | const Eigen::VectorXd&, const std::vector&, const std::vector&, 533 | const Eigen::MatrixXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, double, double, double); 534 | 535 | } -------------------------------------------------------------------------------- /include/common/utilities/CSVTool.h: -------------------------------------------------------------------------------- 1 | #ifndef CSVTOOL_H 2 | #define CSVTOOL_H 3 | 4 | /* 5 | personal tool for .csv reading and modifying. 6 | only suitable for small .csv file. 7 | for large .csv, try (read only) 8 | */ 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include "common/utilities/typeTrans.h" 18 | 19 | enum class FileType{ 20 | READ_WRITE, 21 | CLEAR_DUMP 22 | }; 23 | 24 | class CSVLine{ 25 | public: 26 | // CSVLine(std::string lineTemp, std::streampos filePos); 27 | CSVLine(std::string lineTemp); 28 | CSVLine(std::string label, std::vector values); 29 | ~CSVLine(){} 30 | 31 | // void updateFilePos(std::streampos filePos){_filePos = filePos;} 32 | void getValues(std::vector &values); 33 | void changeValue(std::vector values); 34 | void writeAtEnd(std::fstream &ioStream); 35 | std::string getLabel(){return _label;} 36 | 37 | private: 38 | // std::streampos _filePos; 39 | std::string _label; 40 | std::vector _values; 41 | }; 42 | 43 | /* 44 | FileType::READ_WRITE : must already exist such fileName 45 | FileType::CLEAR_DUMP : if do not exist such file, will create one 46 | */ 47 | class CSVTool{ 48 | public: 49 | CSVTool(std::string fileName, FileType type = FileType::READ_WRITE, int precision = 6); 50 | ~CSVTool(){_ioStream.close();} 51 | 52 | bool getLine(std::string label, std::vector &values); 53 | template 54 | bool getLineDirect(std::string label, Args&... values); 55 | 56 | void modifyLine(std::string label, std::vector &values, bool addNew); 57 | template 58 | void modifyLineDirect(std::string label, bool addNew, Args&... values); 59 | 60 | void readFile(); 61 | void saveFile(); 62 | 63 | bool _hasFile; 64 | private: 65 | std::string _fileName; 66 | std::fstream _ioStream; 67 | int _precision; 68 | std::string _lineTemp; 69 | std::map _labels; 70 | std::vector _lines; 71 | 72 | }; 73 | 74 | /*************************/ 75 | /* CSVLine */ 76 | /*************************/ 77 | // CSVLine::CSVLine(std::string lineTemp, std::streampos filePos) 78 | // :_filePos(filePos){ 79 | 80 | // // std::cout << lineTemp << std::endl; 81 | // } 82 | 83 | inline CSVLine::CSVLine(std::string lineTemp){ 84 | // delete all spaces 85 | lineTemp.erase(std::remove(lineTemp.begin(), lineTemp.end(), ' '), lineTemp.end()); 86 | 87 | std::stringstream ss(lineTemp); 88 | std::string stringTemp; 89 | 90 | std::getline(ss, _label, ','); 91 | 92 | while(std::getline(ss, stringTemp, ',')){ 93 | _values.push_back(stod(stringTemp)); 94 | } 95 | 96 | // std::cout << "**********" << std::endl; 97 | // std::cout << "_label: " << _label << std::fixed << std::setprecision(3) << std::endl; 98 | // for(int i(0); i<_values.size(); ++i){ 99 | // std::cout << _values.at(i) << ",,, "; 100 | // } 101 | // std::cout << std::endl; 102 | } 103 | 104 | inline CSVLine::CSVLine(std::string label, std::vector values) 105 | :_label(label), _values(values){ 106 | 107 | } 108 | 109 | inline void CSVLine::changeValue(std::vector values){ 110 | if(values.size() != _values.size()){ 111 | std::cout << "[WARNING] CSVLine::changeValue, the size changed" << std::endl; 112 | } 113 | _values = values; 114 | } 115 | 116 | inline void CSVLine::getValues(std::vector &values){ 117 | values = _values; 118 | } 119 | 120 | inline void CSVLine::writeAtEnd(std::fstream &ioStream){ 121 | ioStream << _label << ", "; 122 | 123 | for(int i(0); i<_values.size(); ++i){ 124 | ioStream << _values.at(i) << ", "; 125 | } 126 | 127 | ioStream << std::endl; 128 | } 129 | 130 | 131 | /*************************/ 132 | /* CSVTool */ 133 | /*************************/ 134 | inline CSVTool::CSVTool(std::string fileName, FileType type, int precision) 135 | : _fileName(fileName), _precision(precision){ 136 | 137 | if(type == FileType::READ_WRITE){ 138 | _ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out); 139 | 140 | if(!_ioStream.is_open()){ 141 | std::cout << "[ERROR] CSVTool open file: " << fileName << " failed!" << std::endl; 142 | // exit(-1); 143 | _hasFile = false; 144 | }else{ 145 | readFile(); 146 | _hasFile = true; 147 | } 148 | 149 | } 150 | else if(type == FileType::CLEAR_DUMP){ 151 | _ioStream.open(_fileName, std::fstream::out); 152 | } 153 | 154 | } 155 | 156 | inline void CSVTool::readFile(){ 157 | if(!_ioStream.is_open()){ 158 | // _ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out); 159 | std::cout << "[ERROR] CSVTool::readFile, file: " << _fileName << " has not been opened!" << std::endl; 160 | return; 161 | } 162 | 163 | _lines.clear(); 164 | _labels.clear(); 165 | 166 | _ioStream.seekg(0, std::fstream::beg); 167 | size_t lineNum = 0; 168 | while(_ioStream && _ioStream.tellg() != std::fstream::end && getline(_ioStream, _lineTemp)){ 169 | _lines.push_back( new CSVLine(_lineTemp) ); 170 | 171 | if(_labels.count(_lines.at(lineNum)->getLabel()) == 0){ 172 | _labels.insert(std::pair(_lines.at(lineNum)->getLabel(), lineNum)); 173 | ++lineNum; 174 | }else{ 175 | std::cout << "[ERROR] CSVTool::readFile, the label " 176 | << _lines.at(lineNum)->getLabel() << " is repeated" << std::endl; 177 | exit(-1); 178 | } 179 | } 180 | } 181 | 182 | inline bool CSVTool::getLine(std::string label, std::vector &values){ 183 | if(_labels.count(label) == 0){ 184 | std::cout << "[ERROR] No such label: " << label << std::endl; 185 | return false; 186 | }else{ 187 | _lines.at(_labels[label])->getValues(values); 188 | return true; 189 | } 190 | } 191 | 192 | template 193 | inline bool CSVTool::getLineDirect(std::string label, Args&... values){ 194 | std::vector vec; 195 | if(getLine(label, vec)){ 196 | typeTrans::extractVector(vec, values...); 197 | return true; 198 | }else{ 199 | return false; 200 | } 201 | } 202 | 203 | template 204 | inline void CSVTool::modifyLineDirect(std::string label, bool addNew, Args&... values){ 205 | std::vector vec; 206 | typeTrans::combineToVector(vec, values...); 207 | 208 | // std::cout << "CSVTool::modifyLineDirect------" << std::endl; 209 | // std::cout << "label: " << label << std::endl; 210 | // std::cout << "vec: "; 211 | // for(int i(0); iwriteAtEnd(_ioStream); 225 | } 226 | 227 | _ioStream.close(); 228 | _ioStream.open(_fileName, std::fstream::ate | std::fstream::in | std::fstream::out); 229 | } 230 | 231 | inline void CSVTool::modifyLine(std::string label, std::vector &values, bool addNew =false){ 232 | if(_labels.count(label) == 0){ 233 | if(addNew){ 234 | _labels.insert(std::pair(label, _labels.size())); 235 | _lines.push_back(new CSVLine(label, values)); 236 | }else{ 237 | std::cout << "[ERROR] CSVTool::modifyLine, label " << label << "does not exist" << std::endl; 238 | exit(-1); 239 | } 240 | }else{ 241 | _lines.at(_labels[label])->changeValue(values); 242 | } 243 | } 244 | 245 | 246 | #endif // CSVTOOL_H -------------------------------------------------------------------------------- /include/common/utilities/loop.h: -------------------------------------------------------------------------------- 1 | #ifndef _UNITREE_ARM_LOOP_H_ 2 | #define _UNITREE_ARM_LOOP_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "common/utilities/timer.h" 14 | 15 | 16 | // constexpr int THREAD_PRIORITY = 99; // real-time priority 17 | 18 | typedef boost::function Callback; 19 | 20 | class Loop { 21 | public: 22 | Loop(std::string name, float period, int bindCPU = -1); 23 | ~Loop(); 24 | void start(); 25 | void shutdown(); 26 | virtual void functionCB() = 0; 27 | 28 | private: 29 | void entryFunc(); 30 | 31 | std::string _name; 32 | float _period; 33 | int _bindCPU; 34 | bool _bind_cpu_flag = false; 35 | bool _isrunning = false; 36 | std::thread _thread; 37 | 38 | size_t _runTimes = 0; 39 | size_t _timeOutTimes = 0; 40 | 41 | AbsoluteTimer *_timer; 42 | }; 43 | 44 | class LoopFunc : public Loop { 45 | public: 46 | LoopFunc(std::string name, float period, const Callback& _cb) 47 | : Loop(name, period), _fp(_cb){} 48 | LoopFunc(std::string name, float period, int bindCPU, const Callback& _cb) 49 | : Loop(name, period, bindCPU), _fp(_cb){} 50 | void functionCB() { (_fp)(); } 51 | private: 52 | boost::function _fp; 53 | }; 54 | 55 | #endif // _UNITREE_ARM_LOOP_H_ -------------------------------------------------------------------------------- /include/common/utilities/timer.h: -------------------------------------------------------------------------------- 1 | #ifndef _UNITREE_ARM_TIMER_H_ 2 | #define _UNITREE_ARM_TIMER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | inline long long getSystemTime(){ 11 | struct timeval t; 12 | gettimeofday(&t, NULL); 13 | return 1000000 * t.tv_sec + t.tv_usec; 14 | } 15 | 16 | inline double getTimeSecond(){ 17 | double time = getSystemTime() * 0.000001; 18 | return time; 19 | } 20 | 21 | inline void absoluteWait(long long startTime, long long waitTime){ 22 | if(getSystemTime() - startTime > waitTime){ 23 | std::cout << "[WARNING] The waitTime=" << waitTime << " of function absoluteWait is not enough!" << std::endl 24 | << "The program has already cost " << getSystemTime() - startTime << "us." << std::endl; 25 | } 26 | while(getSystemTime() - startTime < waitTime){ 27 | usleep(50); 28 | } 29 | } 30 | 31 | 32 | /* 33 | waitTimeS = 0 means do not care time out 34 | */ 35 | class AbsoluteTimer{ 36 | public: 37 | AbsoluteTimer(double waitTimeS); 38 | ~AbsoluteTimer(); 39 | void start(); 40 | bool wait(); 41 | private: 42 | void _updateWaitTime(double waitTimeS); 43 | int _timerFd; 44 | uint64_t _missed; 45 | double _waitTime; 46 | double _startTime; 47 | double _leftTime; 48 | double _nextWaitTime; 49 | itimerspec _timerSpec; 50 | }; 51 | 52 | #endif -------------------------------------------------------------------------------- /include/common/utilities/typeTrans.h: -------------------------------------------------------------------------------- 1 | #ifndef TYPETRANS_H 2 | #define TYPETRANS_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace typeTrans{ 9 | 10 | inline void addValue(std::vector &vec, double value){ 11 | vec.push_back(value); 12 | } 13 | 14 | inline double getValue(std::vector &vec, double value){ 15 | value = vec.at(0); 16 | std::vector::iterator begin = vec.begin(); 17 | vec.erase(begin); 18 | 19 | return value; 20 | } 21 | 22 | inline void addValue(std::vector &vec, Eigen::MatrixXd value){ 23 | for(int i(0); i &vec, Eigen::MatrixXd value){ 31 | std::vector::iterator begin = vec.begin(); 32 | std::vector::iterator end = begin; 33 | 34 | for(int i(0); i 48 | inline void combineToVector(std::vector &vec, T value){ 49 | addValue(vec, value); 50 | } 51 | 52 | template 53 | inline void combineToVector(std::vector &vec, const T t, const Args... rest){ 54 | combineToVector(vec, t); 55 | 56 | combineToVector(vec, rest...); 57 | } 58 | 59 | 60 | /* extract different type variables from vector */ 61 | template 62 | inline void extractVector(std::vector &vec, T &value){ 63 | value = getValue(vec, value); 64 | } 65 | 66 | template 67 | inline void extractVector(std::vector &vec, T &t, Args&... rest){ 68 | extractVector(vec, t); 69 | 70 | extractVector(vec, rest...); 71 | } 72 | 73 | } 74 | 75 | #endif // TYPETRANS_H -------------------------------------------------------------------------------- /include/control/CtrlComponents.h: -------------------------------------------------------------------------------- 1 | #ifndef CTRLCOMPONENTS_H 2 | #define CTRLCOMPONENTS_H 3 | 4 | #include 5 | #include 6 | #include "common/utilities/loop.h" 7 | #include "message/arm_common.h" 8 | #include "message/LowlevelCmd.h" 9 | #include "message/LowlevelState.h" 10 | #include "common/utilities/CSVTool.h" 11 | #include "model/ArmModel.h" 12 | #include "interface/IOUDP.h" 13 | #include "control/armSDK.h" 14 | #include "model/unitree_gripper.h" 15 | 16 | using namespace std; 17 | 18 | struct CtrlComponents{ 19 | public: 20 | CtrlComponents(int argc, char**argv); 21 | ~CtrlComponents(); 22 | 23 | std::string armConfigPath; 24 | CmdPanel *cmdPanel; 25 | IOInterface *ioInter; 26 | Z1Model *armModel; 27 | CSVTool *stateCSV; 28 | std::shared_ptr gripper; 29 | double gripper_max_tau = 10; 30 | 31 | SendCmd sendCmd; // cmd that receive from SDK 32 | RecvState recvState;// state that send to SDK 33 | 34 | //config 35 | double dt; 36 | bool *running; 37 | Control ctrl = Control::SDK; 38 | bool hasGripper; 39 | bool isCollisionOpen; 40 | double collisionTLimit; 41 | bool isPlot; 42 | int trajChoose = 1; 43 | size_t armType = 36; 44 | std::string ctrl_IP; 45 | uint ctrl_port; 46 | 47 | void geneObj(); 48 | void writeData(); 49 | private: 50 | void configProcess(int argc, char** argv); 51 | 52 | double _loadWeight; 53 | }; 54 | 55 | #endif // CTRLCOMPONENTS_H -------------------------------------------------------------------------------- /include/control/armSDK.h: -------------------------------------------------------------------------------- 1 | #ifndef _SDK_H 2 | #define _SDK_H 3 | 4 | #include "message/udp.h" 5 | #include "control/cmdPanel.h" 6 | 7 | class ARMSDK : public CmdPanel{ 8 | public: 9 | ARMSDK(std::vector events, 10 | EmptyAction emptyAction, const char* sdkIP, uint sdkPort, uint ownPort, double dt = 0.002); 11 | ~ARMSDK(); 12 | SendCmd getSendCmd(); 13 | int getState(size_t channelID = 0); 14 | void setRecvState(RecvState& recvState); 15 | void start(); 16 | private: 17 | void _sendRecv(); 18 | void _read(){}; 19 | UDPPort *_udp; 20 | SendCmd _sendCmd, _sendCmdTemp; 21 | RecvState _recvState; 22 | size_t _recvLength; 23 | }; 24 | 25 | #endif -------------------------------------------------------------------------------- /include/control/cmdPanel.h: -------------------------------------------------------------------------------- 1 | #ifndef _CMDPANEL_H_ 2 | #define _CMDPANEL_H_ 3 | 4 | #include 5 | #include 6 | #include "common/utilities/loop.h" 7 | #include "message/arm_common.h" 8 | 9 | 10 | enum class KeyPress{ 11 | RELEASE, 12 | PRESS, 13 | REPEAT 14 | }; 15 | 16 | 17 | enum class ActionType{ 18 | EMPTY, 19 | STATE, 20 | VALUE 21 | }; 22 | 23 | 24 | struct KeyCmd{ 25 | std::string c; 26 | KeyPress keyPress; 27 | }; 28 | 29 | 30 | class KeyAction{ 31 | public: 32 | KeyAction(ActionType type); 33 | virtual ~KeyAction(){}; 34 | ActionType getType(){return _type;} 35 | protected: 36 | ActionType _type; 37 | }; 38 | 39 | 40 | class StateAction : public KeyAction{ 41 | public: 42 | StateAction(std::string c, int state, KeyPress press = KeyPress::PRESS); 43 | virtual ~StateAction(){}; 44 | int getState(){return _state;}; 45 | bool handleCmd(KeyCmd keyCmd, int &state); 46 | protected: 47 | int _state; 48 | KeyCmd _keyCmdSet; 49 | }; 50 | 51 | class EmptyAction : public StateAction{ 52 | public: 53 | EmptyAction(int state); 54 | ~EmptyAction(){}; 55 | private: 56 | 57 | }; 58 | 59 | class ValueAction : public KeyAction{ 60 | public: 61 | ValueAction(std::string cUp, std::string cDown, double deltaValue, double initValue = 0.0); 62 | ValueAction(std::string cUp, std::string cDown, std::string cGoZero, double deltaValue, double initValue = 0.0); 63 | ValueAction(std::string cUp, std::string cDown, double deltaValue, double limit1, double limit2, double initValue = 0.0); 64 | ValueAction(std::string cUp, std::string cDown, std::string cGoZero, double deltaValue, double limit1, double limit2, double initValue = 0.0); 65 | 66 | ~ValueAction(){}; 67 | bool handleCmd(KeyCmd keyCmd); 68 | void setDt(double dt); 69 | double getValue(); 70 | double getDValue(); 71 | double getDirection(){return _changeDirection;}; 72 | void setValue(double value){_value = value;} 73 | private: 74 | double _value; 75 | double _changeDirection=0.0; 76 | double _dV = 0.0; //delta value per delta time 77 | double _dt = 0.0; 78 | double _dVdt = 0.0; // delta value per second 79 | double _dVdtf = 0.0; // delta value per second after fliter 80 | double _lim1, _lim2; 81 | bool _hasLim = false; 82 | bool _hasGoZero = false; 83 | 84 | KeyCmd _upCmdSet; 85 | KeyCmd _downCmdSet; 86 | KeyCmd _goZeroCmdSet; 87 | 88 | }; 89 | 90 | 91 | class CmdPanel{ 92 | public: 93 | CmdPanel(std::vector events, 94 | EmptyAction emptyAction, size_t channelNum = 1, double dt = 0.002); 95 | virtual ~CmdPanel(); 96 | virtual int getState(size_t channelID = 0); 97 | std::vector getValues() {return _values;}; 98 | std::vector getDValues() {return _dValues;}; 99 | std::vector getDirections() {return _changeDirections;}; 100 | void setValue(std::vector values); 101 | void setValue(double value, size_t id); 102 | virtual std::string getString(std::string slogan); 103 | virtual std::vector stringToArray(std::string slogan); 104 | virtual std::vector > stringToMatrix(std::string slogan); 105 | virtual SendCmd getSendCmd(); 106 | virtual void setRecvState(RecvState& recvState){}; 107 | void start(){_runThread->start();} 108 | protected: 109 | virtual void _read() = 0; 110 | void _run(); 111 | void _updateState(); 112 | void _pressKeyboard(); 113 | void _releaseKeyboard(); 114 | 115 | LoopFunc *_runThread; 116 | LoopFunc *_readThread; 117 | 118 | std::vector _stateEvents; 119 | std::vector _valueEvents; 120 | 121 | EmptyAction _emptyAction; 122 | size_t _actionNum = 0; 123 | size_t _stateNum = 0; 124 | size_t _valueNum = 0; 125 | size_t _channelNum; 126 | std::vector _values; 127 | std::vector _dValues; 128 | std::vector _changeDirections; 129 | int _state; 130 | std::vector> _stateQueue; 131 | std::vector _outputState; 132 | std::vector _getState; 133 | double _dt; 134 | KeyCmd _keyCmd; 135 | std::string _cPast = ""; 136 | 137 | bool _running = true; 138 | }; 139 | 140 | #endif -------------------------------------------------------------------------------- /include/control/keyboard.h: -------------------------------------------------------------------------------- 1 | #ifndef _UNITREE_ARM_KEYBOARD_H_ 2 | #define _UNITREE_ARM_KEYBOARD_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "message/udp.h" 9 | #include "common/enumClass.h" 10 | #include "control/cmdPanel.h" 11 | 12 | class Keyboard : public CmdPanel{ 13 | public: 14 | Keyboard(std::vector events, 15 | EmptyAction emptyAction, size_t channelNum = 1, double dt = 0.002); 16 | ~Keyboard(); 17 | std::string getString(std::string slogan); 18 | std::vector stringToArray(std::string slogan); 19 | std::vector > stringToMatrix(std::string slogan); 20 | private: 21 | void _read(); 22 | void _pauseKey(); 23 | void _startKey(); 24 | void _extractCmd(); 25 | 26 | fd_set _set; 27 | char _c = '\0'; 28 | 29 | termios _oldSettings; 30 | termios _newSettings; 31 | timeval _tv; 32 | }; 33 | 34 | #endif // _UNITREE_ARM_KEYBOARD_H_ -------------------------------------------------------------------------------- /include/interface/IOInterface.h: -------------------------------------------------------------------------------- 1 | #ifndef IOINTERFACE_H 2 | #define IOINTERFACE_H 3 | 4 | #include 5 | #include "message/LowlevelCmd.h" 6 | #include "message/LowlevelState.h" 7 | #include "control/keyboard.h" 8 | #include "common/math/robotics.h" 9 | 10 | class IOInterface{ 11 | public: 12 | IOInterface(){} 13 | ~IOInterface(){ 14 | delete lowCmd; 15 | delete lowState; 16 | }; 17 | virtual bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state) = 0; 18 | virtual bool calibration(){return false;}; 19 | bool checkGripper(){return hasGripper;}; 20 | LowlevelCmd *lowCmd; 21 | LowlevelState *lowState; 22 | virtual bool isDisconnect(){ return false;}; 23 | bool hasErrorState; 24 | protected: 25 | bool hasGripper; 26 | }; 27 | 28 | #endif //IOINTERFACE_H -------------------------------------------------------------------------------- /include/interface/IOUDP.h: -------------------------------------------------------------------------------- 1 | #ifndef IOUDP_H 2 | #define IOUDP_H 3 | 4 | #include "interface/IOInterface.h" 5 | 6 | class IOUDP : public IOInterface{ 7 | public: 8 | IOUDP(const char* IP, uint port, size_t timeOutUs = 100000, bool showInfo = true); 9 | ~IOUDP(); 10 | 11 | bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state); 12 | bool calibration(); 13 | bool isDisconnect(){ return _ioPort->isDisConnect;} 14 | private: 15 | IOPort *_ioPort; 16 | 17 | UDPSendCmd _cmd; 18 | UDPRecvState _state; 19 | UDPRecvStateOld _stateOld; 20 | size_t _motorNum; 21 | size_t _jointNum; 22 | uint8_t _singleState; 23 | uint8_t _selfCheck[10]; 24 | }; 25 | 26 | #endif // IOUDP_H -------------------------------------------------------------------------------- /include/message/LowlevelCmd.h: -------------------------------------------------------------------------------- 1 | #ifndef LOWLEVELCMD_H 2 | #define LOWLEVELCMD_H 3 | 4 | #include "common/math/mathTypes.h" 5 | #include "common/math/mathTools.h" 6 | #include 7 | #include 8 | 9 | struct LowlevelCmd{ 10 | public: 11 | std::vector q; 12 | std::vector dq; 13 | std::vector tau; 14 | std::vector kp; 15 | std::vector kd; 16 | 17 | std::vector> q_data; 18 | std::vector> dq_data; 19 | std::vector> tauf_data; 20 | std::vector> tau_data; 21 | 22 | LowlevelCmd(); 23 | ~LowlevelCmd(){} 24 | 25 | void setZeroDq(); 26 | void setZeroTau(); 27 | void setZeroKp(); 28 | void setZeroKd(); 29 | void setQ(VecX qInput); 30 | void setQd(VecX qDInput); 31 | void setTau(VecX tauInput); 32 | 33 | void setControlGain(); 34 | void setControlGain(std::vector KP, std::vector KW); 35 | void setPassive(); 36 | 37 | void setGripperGain(); 38 | void setGripperGain(float KP, float KW); 39 | void setGripperZeroGain(); 40 | void setGripperQ(double qInput); 41 | double getGripperQ(); 42 | void setGripperQd(double qdInput); 43 | double getGripperQd(); 44 | void setGripperTau(double tauInput); 45 | double getGripperTau(); 46 | 47 | Vec6 getQ(); 48 | Vec6 getQd(); 49 | 50 | void resizeGripper(); 51 | private: 52 | size_t _dof = 6; 53 | }; 54 | 55 | 56 | #endif //LOWLEVELCMD_H 57 | -------------------------------------------------------------------------------- /include/message/LowlevelState.h: -------------------------------------------------------------------------------- 1 | #ifndef LOWLEVELSTATE_HPP 2 | #define LOWLEVELSTATE_HPP 3 | 4 | #include 5 | #include 6 | #include "common/math/mathTools.h" 7 | #include "common/enumClass.h" 8 | #include "common/math/Filter.h" 9 | 10 | struct LowlevelState{ 11 | public: 12 | LowlevelState(double dt); 13 | ~LowlevelState(); 14 | 15 | std::vector q; 16 | std::vector dq; 17 | std::vector ddq; 18 | std::vector tau; 19 | 20 | std::vector> q_data; 21 | std::vector> dq_data; 22 | std::vector> ddq_data; 23 | std::vector> tau_data; 24 | 25 | std::vector temperature; 26 | std::vector errorstate; 27 | std::vector isMotorConnected; 28 | 29 | std::vector qFiltered; 30 | std::vector dqFiltered; 31 | std::vector ddqFiltered; 32 | std::vector tauFiltered; 33 | 34 | LPFilter *qFilter; 35 | LPFilter *dqFilter; 36 | LPFilter *ddqFilter; 37 | LPFilter *tauFilter; 38 | 39 | void resizeGripper(double dt); 40 | void runFilter(); 41 | bool checkError(); 42 | Vec6 getQ(); 43 | Vec6 getQd(); 44 | Vec6 getQdd(); 45 | Vec6 getTau(); 46 | Vec6 getQFiltered(); 47 | Vec6 getQdFiltered(); 48 | Vec6 getQddFiltered(); 49 | Vec6 getTauFiltered(); 50 | double getGripperQ(); 51 | double getGripperQd(); 52 | double getGripperTau(); 53 | double getGripperTauFiltered(); 54 | private: 55 | size_t _dof = 6; 56 | int temporatureLimit = 80;// centigrade 57 | std::vector _isMotorConnectedCnt; 58 | std::vector _isMotorLostConnection; 59 | }; 60 | 61 | #endif //LOWLEVELSTATE_HPP 62 | -------------------------------------------------------------------------------- /include/message/MotorCmd.h: -------------------------------------------------------------------------------- 1 | // Generated by gencpp from file unitree_legged_msgs/MotorCmd.msg 2 | // DO NOT EDIT! 3 | 4 | 5 | #ifndef UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_H 6 | #define UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_H 7 | 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | namespace unitree_legged_msgs 20 | { 21 | template 22 | struct MotorCmd_ 23 | { 24 | typedef MotorCmd_ Type; 25 | 26 | MotorCmd_() 27 | : mode(0) 28 | , q(0.0) 29 | , dq(0.0) 30 | , tau(0.0) 31 | , Kp(0.0) 32 | , Kd(0.0) 33 | , reserve() { 34 | reserve.assign(0); 35 | } 36 | MotorCmd_(const ContainerAllocator& _alloc) 37 | : mode(0) 38 | , q(0.0) 39 | , dq(0.0) 40 | , tau(0.0) 41 | , Kp(0.0) 42 | , Kd(0.0) 43 | , reserve() { 44 | (void)_alloc; 45 | reserve.assign(0); 46 | } 47 | 48 | 49 | 50 | typedef uint8_t _mode_type; 51 | _mode_type mode; 52 | 53 | typedef float _q_type; 54 | _q_type q; 55 | 56 | typedef float _dq_type; 57 | _dq_type dq; 58 | 59 | typedef float _tau_type; 60 | _tau_type tau; 61 | 62 | typedef float _Kp_type; 63 | _Kp_type Kp; 64 | 65 | typedef float _Kd_type; 66 | _Kd_type Kd; 67 | 68 | typedef boost::array _reserve_type; 69 | _reserve_type reserve; 70 | 71 | 72 | 73 | 74 | 75 | typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd_ > Ptr; 76 | typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd_ const> ConstPtr; 77 | 78 | }; // struct MotorCmd_ 79 | 80 | typedef ::unitree_legged_msgs::MotorCmd_ > MotorCmd; 81 | 82 | typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd > MotorCmdPtr; 83 | typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd const> MotorCmdConstPtr; 84 | 85 | // constants requiring out of line definition 86 | 87 | 88 | 89 | template 90 | std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::MotorCmd_ & v) 91 | { 92 | ros::message_operations::Printer< ::unitree_legged_msgs::MotorCmd_ >::stream(s, "", v); 93 | return s; 94 | } 95 | 96 | 97 | template 98 | bool operator==(const ::unitree_legged_msgs::MotorCmd_ & lhs, const ::unitree_legged_msgs::MotorCmd_ & rhs) 99 | { 100 | return lhs.mode == rhs.mode && 101 | lhs.q == rhs.q && 102 | lhs.dq == rhs.dq && 103 | lhs.tau == rhs.tau && 104 | lhs.Kp == rhs.Kp && 105 | lhs.Kd == rhs.Kd && 106 | lhs.reserve == rhs.reserve; 107 | } 108 | 109 | template 110 | bool operator!=(const ::unitree_legged_msgs::MotorCmd_ & lhs, const ::unitree_legged_msgs::MotorCmd_ & rhs) 111 | { 112 | return !(lhs == rhs); 113 | } 114 | 115 | 116 | } // namespace unitree_legged_msgs 117 | 118 | namespace ros 119 | { 120 | namespace message_traits 121 | { 122 | 123 | 124 | 125 | 126 | 127 | template 128 | struct IsFixedSize< ::unitree_legged_msgs::MotorCmd_ > 129 | : TrueType 130 | { }; 131 | 132 | template 133 | struct IsFixedSize< ::unitree_legged_msgs::MotorCmd_ const> 134 | : TrueType 135 | { }; 136 | 137 | template 138 | struct IsMessage< ::unitree_legged_msgs::MotorCmd_ > 139 | : TrueType 140 | { }; 141 | 142 | template 143 | struct IsMessage< ::unitree_legged_msgs::MotorCmd_ const> 144 | : TrueType 145 | { }; 146 | 147 | template 148 | struct HasHeader< ::unitree_legged_msgs::MotorCmd_ > 149 | : FalseType 150 | { }; 151 | 152 | template 153 | struct HasHeader< ::unitree_legged_msgs::MotorCmd_ const> 154 | : FalseType 155 | { }; 156 | 157 | 158 | template 159 | struct MD5Sum< ::unitree_legged_msgs::MotorCmd_ > 160 | { 161 | static const char* value() 162 | { 163 | return "bbb3b7d91319c3a1b99055f0149ba221"; 164 | } 165 | 166 | static const char* value(const ::unitree_legged_msgs::MotorCmd_&) { return value(); } 167 | static const uint64_t static_value1 = 0xbbb3b7d91319c3a1ULL; 168 | static const uint64_t static_value2 = 0xb99055f0149ba221ULL; 169 | }; 170 | 171 | template 172 | struct DataType< ::unitree_legged_msgs::MotorCmd_ > 173 | { 174 | static const char* value() 175 | { 176 | return "unitree_legged_msgs/MotorCmd"; 177 | } 178 | 179 | static const char* value(const ::unitree_legged_msgs::MotorCmd_&) { return value(); } 180 | }; 181 | 182 | template 183 | struct Definition< ::unitree_legged_msgs::MotorCmd_ > 184 | { 185 | static const char* value() 186 | { 187 | return "uint8 mode # motor target mode\n" 188 | "float32 q # motor target position\n" 189 | "float32 dq # motor target velocity\n" 190 | "float32 tau # motor target torque\n" 191 | "float32 Kp # motor spring stiffness coefficient\n" 192 | "float32 Kd # motor damper coefficient\n" 193 | "uint32[3] reserve # motor target torque\n" 194 | ; 195 | } 196 | 197 | static const char* value(const ::unitree_legged_msgs::MotorCmd_&) { return value(); } 198 | }; 199 | 200 | } // namespace message_traits 201 | } // namespace ros 202 | 203 | namespace ros 204 | { 205 | namespace serialization 206 | { 207 | 208 | template struct Serializer< ::unitree_legged_msgs::MotorCmd_ > 209 | { 210 | template inline static void allInOne(Stream& stream, T m) 211 | { 212 | stream.next(m.mode); 213 | stream.next(m.q); 214 | stream.next(m.dq); 215 | stream.next(m.tau); 216 | stream.next(m.Kp); 217 | stream.next(m.Kd); 218 | stream.next(m.reserve); 219 | } 220 | 221 | ROS_DECLARE_ALLINONE_SERIALIZER 222 | }; // struct MotorCmd_ 223 | 224 | } // namespace serialization 225 | } // namespace ros 226 | 227 | namespace ros 228 | { 229 | namespace message_operations 230 | { 231 | 232 | template 233 | struct Printer< ::unitree_legged_msgs::MotorCmd_ > 234 | { 235 | template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::MotorCmd_& v) 236 | { 237 | s << indent << "mode: "; 238 | Printer::stream(s, indent + " ", v.mode); 239 | s << indent << "q: "; 240 | Printer::stream(s, indent + " ", v.q); 241 | s << indent << "dq: "; 242 | Printer::stream(s, indent + " ", v.dq); 243 | s << indent << "tau: "; 244 | Printer::stream(s, indent + " ", v.tau); 245 | s << indent << "Kp: "; 246 | Printer::stream(s, indent + " ", v.Kp); 247 | s << indent << "Kd: "; 248 | Printer::stream(s, indent + " ", v.Kd); 249 | s << indent << "reserve[]" << std::endl; 250 | for (size_t i = 0; i < v.reserve.size(); ++i) 251 | { 252 | s << indent << " reserve[" << i << "]: "; 253 | Printer::stream(s, indent + " ", v.reserve[i]); 254 | } 255 | } 256 | }; 257 | 258 | } // namespace message_operations 259 | } // namespace ros 260 | 261 | #endif // UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_H 262 | -------------------------------------------------------------------------------- /include/message/MotorState.h: -------------------------------------------------------------------------------- 1 | // Generated by gencpp from file unitree_legged_msgs/MotorState.msg 2 | // DO NOT EDIT! 3 | 4 | 5 | #ifndef UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_H 6 | #define UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_H 7 | 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | namespace unitree_legged_msgs 20 | { 21 | template 22 | struct MotorState_ 23 | { 24 | typedef MotorState_ Type; 25 | 26 | MotorState_() 27 | : mode(0) 28 | , q(0.0) 29 | , dq(0.0) 30 | , ddq(0.0) 31 | , tauEst(0.0) 32 | , q_raw(0.0) 33 | , dq_raw(0.0) 34 | , ddq_raw(0.0) 35 | , temperature(0) 36 | , reserve() { 37 | reserve.assign(0); 38 | } 39 | MotorState_(const ContainerAllocator& _alloc) 40 | : mode(0) 41 | , q(0.0) 42 | , dq(0.0) 43 | , ddq(0.0) 44 | , tauEst(0.0) 45 | , q_raw(0.0) 46 | , dq_raw(0.0) 47 | , ddq_raw(0.0) 48 | , temperature(0) 49 | , reserve() { 50 | (void)_alloc; 51 | reserve.assign(0); 52 | } 53 | 54 | 55 | 56 | typedef uint8_t _mode_type; 57 | _mode_type mode; 58 | 59 | typedef float _q_type; 60 | _q_type q; 61 | 62 | typedef float _dq_type; 63 | _dq_type dq; 64 | 65 | typedef float _ddq_type; 66 | _ddq_type ddq; 67 | 68 | typedef float _tauEst_type; 69 | _tauEst_type tauEst; 70 | 71 | typedef float _q_raw_type; 72 | _q_raw_type q_raw; 73 | 74 | typedef float _dq_raw_type; 75 | _dq_raw_type dq_raw; 76 | 77 | typedef float _ddq_raw_type; 78 | _ddq_raw_type ddq_raw; 79 | 80 | typedef int8_t _temperature_type; 81 | _temperature_type temperature; 82 | 83 | typedef boost::array _reserve_type; 84 | _reserve_type reserve; 85 | 86 | 87 | 88 | 89 | 90 | typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState_ > Ptr; 91 | typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState_ const> ConstPtr; 92 | 93 | }; // struct MotorState_ 94 | 95 | typedef ::unitree_legged_msgs::MotorState_ > MotorState; 96 | 97 | typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState > MotorStatePtr; 98 | typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState const> MotorStateConstPtr; 99 | 100 | // constants requiring out of line definition 101 | 102 | 103 | 104 | template 105 | std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::MotorState_ & v) 106 | { 107 | ros::message_operations::Printer< ::unitree_legged_msgs::MotorState_ >::stream(s, "", v); 108 | return s; 109 | } 110 | 111 | 112 | template 113 | bool operator==(const ::unitree_legged_msgs::MotorState_ & lhs, const ::unitree_legged_msgs::MotorState_ & rhs) 114 | { 115 | return lhs.mode == rhs.mode && 116 | lhs.q == rhs.q && 117 | lhs.dq == rhs.dq && 118 | lhs.ddq == rhs.ddq && 119 | lhs.tauEst == rhs.tauEst && 120 | lhs.q_raw == rhs.q_raw && 121 | lhs.dq_raw == rhs.dq_raw && 122 | lhs.ddq_raw == rhs.ddq_raw && 123 | lhs.temperature == rhs.temperature && 124 | lhs.reserve == rhs.reserve; 125 | } 126 | 127 | template 128 | bool operator!=(const ::unitree_legged_msgs::MotorState_ & lhs, const ::unitree_legged_msgs::MotorState_ & rhs) 129 | { 130 | return !(lhs == rhs); 131 | } 132 | 133 | 134 | } // namespace unitree_legged_msgs 135 | 136 | namespace ros 137 | { 138 | namespace message_traits 139 | { 140 | 141 | 142 | 143 | 144 | 145 | template 146 | struct IsFixedSize< ::unitree_legged_msgs::MotorState_ > 147 | : TrueType 148 | { }; 149 | 150 | template 151 | struct IsFixedSize< ::unitree_legged_msgs::MotorState_ const> 152 | : TrueType 153 | { }; 154 | 155 | template 156 | struct IsMessage< ::unitree_legged_msgs::MotorState_ > 157 | : TrueType 158 | { }; 159 | 160 | template 161 | struct IsMessage< ::unitree_legged_msgs::MotorState_ const> 162 | : TrueType 163 | { }; 164 | 165 | template 166 | struct HasHeader< ::unitree_legged_msgs::MotorState_ > 167 | : FalseType 168 | { }; 169 | 170 | template 171 | struct HasHeader< ::unitree_legged_msgs::MotorState_ const> 172 | : FalseType 173 | { }; 174 | 175 | 176 | template 177 | struct MD5Sum< ::unitree_legged_msgs::MotorState_ > 178 | { 179 | static const char* value() 180 | { 181 | return "94c55ee3b7852be2bd437b20ce12a254"; 182 | } 183 | 184 | static const char* value(const ::unitree_legged_msgs::MotorState_&) { return value(); } 185 | static const uint64_t static_value1 = 0x94c55ee3b7852be2ULL; 186 | static const uint64_t static_value2 = 0xbd437b20ce12a254ULL; 187 | }; 188 | 189 | template 190 | struct DataType< ::unitree_legged_msgs::MotorState_ > 191 | { 192 | static const char* value() 193 | { 194 | return "unitree_legged_msgs/MotorState"; 195 | } 196 | 197 | static const char* value(const ::unitree_legged_msgs::MotorState_&) { return value(); } 198 | }; 199 | 200 | template 201 | struct Definition< ::unitree_legged_msgs::MotorState_ > 202 | { 203 | static const char* value() 204 | { 205 | return "uint8 mode # motor current mode \n" 206 | "float32 q # motor current position（rad）\n" 207 | "float32 dq # motor current speed（rad/s）\n" 208 | "float32 ddq # motor current speed（rad/s）\n" 209 | "float32 tauEst # current estimated output torque（N*m）\n" 210 | "float32 q_raw # motor current position（rad）\n" 211 | "float32 dq_raw # motor current speed（rad/s）\n" 212 | "float32 ddq_raw # motor current speed（rad/s）\n" 213 | "int8 temperature # motor temperature（slow conduction of temperature leads to lag）\n" 214 | "uint32[2] reserve\n" 215 | ; 216 | } 217 | 218 | static const char* value(const ::unitree_legged_msgs::MotorState_&) { return value(); } 219 | }; 220 | 221 | } // namespace message_traits 222 | } // namespace ros 223 | 224 | namespace ros 225 | { 226 | namespace serialization 227 | { 228 | 229 | template struct Serializer< ::unitree_legged_msgs::MotorState_ > 230 | { 231 | template inline static void allInOne(Stream& stream, T m) 232 | { 233 | stream.next(m.mode); 234 | stream.next(m.q); 235 | stream.next(m.dq); 236 | stream.next(m.ddq); 237 | stream.next(m.tauEst); 238 | stream.next(m.q_raw); 239 | stream.next(m.dq_raw); 240 | stream.next(m.ddq_raw); 241 | stream.next(m.temperature); 242 | stream.next(m.reserve); 243 | } 244 | 245 | ROS_DECLARE_ALLINONE_SERIALIZER 246 | }; // struct MotorState_ 247 | 248 | } // namespace serialization 249 | } // namespace ros 250 | 251 | namespace ros 252 | { 253 | namespace message_operations 254 | { 255 | 256 | template 257 | struct Printer< ::unitree_legged_msgs::MotorState_ > 258 | { 259 | template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::MotorState_& v) 260 | { 261 | s << indent << "mode: "; 262 | Printer::stream(s, indent + " ", v.mode); 263 | s << indent << "q: "; 264 | Printer::stream(s, indent + " ", v.q); 265 | s << indent << "dq: "; 266 | Printer::stream(s, indent + " ", v.dq); 267 | s << indent << "ddq: "; 268 | Printer::stream(s, indent + " ", v.ddq); 269 | s << indent << "tauEst: "; 270 | Printer::stream(s, indent + " ", v.tauEst); 271 | s << indent << "q_raw: "; 272 | Printer::stream(s, indent + " ", v.q_raw); 273 | s << indent << "dq_raw: "; 274 | Printer::stream(s, indent + " ", v.dq_raw); 275 | s << indent << "ddq_raw: "; 276 | Printer::stream(s, indent + " ", v.ddq_raw); 277 | s << indent << "temperature: "; 278 | Printer::stream(s, indent + " ", v.temperature); 279 | s << indent << "reserve[]" << std::endl; 280 | for (size_t i = 0; i < v.reserve.size(); ++i) 281 | { 282 | s << indent << " reserve[" << i << "]: "; 283 | Printer::stream(s, indent + " ", v.reserve[i]); 284 | } 285 | } 286 | }; 287 | 288 | } // namespace message_operations 289 | } // namespace ros 290 | 291 | #endif // UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_H 292 | -------------------------------------------------------------------------------- /include/message/arm_common.h: -------------------------------------------------------------------------------- 1 | #ifndef _UNITREE_ARM_ARM_COMMON_H_ 2 | #define _UNITREE_ARM_ARM_COMMON_H_ 3 | 4 | #include 5 | 6 | #pragma pack(1) 7 | 8 | // 4 Byte 9 | enum class ArmFSMState{ 10 | INVALID, 11 | PASSIVE, 12 | JOINTCTRL, 13 | CARTESIAN, 14 | MOVEJ, 15 | MOVEL, 16 | MOVEC, 17 | TRAJECTORY, 18 | TOSTATE, 19 | SAVESTATE, 20 | TEACH, 21 | TEACHREPEAT, 22 | CALIBRATION, 23 | SETTRAJ,//no longer used 24 | BACKTOSTART, 25 | NEXT, 26 | LOWCMD 27 | }; 28 | 29 | enum class TrajType{ 30 | MoveJ, 31 | MoveL, 32 | MoveC, 33 | Stop 34 | }; 35 | 36 | // 20 Byte 37 | struct JointCmd{ 38 | float T; 39 | float W; 40 | float Pos; 41 | float K_P; 42 | float K_W; 43 | }; 44 | 45 | typedef struct{ 46 | uint8_t reserved : 6 ; 47 | uint8_t state : 2 ;//whether motor is connected; 0-ok, 1-disconnected, 2-CRC error 48 | }Motor_Connected; 49 | 50 | typedef struct{ 51 | int8_t temperature; 52 | /* 0x01: phase current is too large 53 | * 0x02: phase leakage 54 | * 0x04: overheat(including the motor windings and the motor shell) 55 | * 0x20: jumped 56 | */ 57 | uint8_t error; 58 | Motor_Connected isConnected; 59 | }Motor_State; 60 | 61 | struct JointStateOld{//no error state return 62 | float T; 63 | float W; 64 | float Acc; 65 | float Pos; 66 | }; 67 | 68 | struct JointState{ 69 | float T; 70 | float W; 71 | float Acc; 72 | float Pos; 73 | Motor_State state[2]; 74 | }; 75 | 76 | //140bytes 77 | union UDPSendCmd{ 78 | uint8_t checkCmd; 79 | JointCmd jointCmd[7]; 80 | }; 81 | 82 | struct UDPRecvStateOld{ 83 | JointStateOld jointStateOld[7]; 84 | }; 85 | 86 | struct UDPRecvState{ 87 | JointState jointState[7]; 88 | }; 89 | 90 | struct Posture{ 91 | double roll; 92 | double pitch; 93 | double yaw; 94 | double x; 95 | double y; 96 | double z; 97 | }; 98 | 99 | struct TrajCmd{ 100 | TrajType trajType; 101 | Posture posture[2]; 102 | double gripperPos; 103 | double maxSpeed; 104 | double stopTime; 105 | int trajOrder; 106 | }; 107 | 108 | union ValueUnion{ 109 | char name[10]; 110 | JointCmd jointCmd[7]; 111 | TrajCmd trajCmd; 112 | }; 113 | 114 | struct SendCmd{ 115 | uint8_t head[2]; 116 | ArmFSMState state; 117 | bool track;// whether let arm track jointCmd in State_JOINTCTRL or posture[0] in State_CARTESIAN 118 | ValueUnion valueUnion; 119 | }; 120 | 121 | 122 | struct RecvState{ 123 | uint8_t head[2]; 124 | ArmFSMState state; 125 | JointState jointState[7]; 126 | Posture cartesianState; 127 | }; 128 | 129 | constexpr int SENDCMD_LENGTH = (sizeof(SendCmd)); 130 | constexpr int RECVSTATE_LENGTH = (sizeof(RecvState)); 131 | constexpr int JointCmd_LENGTH = (sizeof(JointCmd)); 132 | constexpr int JointState_LENGTH = (sizeof(JointState)); 133 | constexpr int JointStateOld_LENGTH = (sizeof(JointStateOld)); 134 | 135 | #pragma pack() 136 | 137 | #endif // _UNITREE_ARM_ARM_MSG_H_ -------------------------------------------------------------------------------- /include/message/udp.h: -------------------------------------------------------------------------------- 1 | #ifndef _UDP_H 2 | #define _UDP_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "common/utilities/timer.h" 10 | 11 | enum class BlockYN{ 12 | YES, 13 | NO 14 | }; 15 | 16 | 17 | class IOPort{ 18 | public: 19 | IOPort(std::string name, BlockYN blockYN, size_t recvLength, size_t timeOutUs, bool showInfo) 20 | :_name(name){ 21 | resetIO(blockYN, recvLength, timeOutUs, showInfo); 22 | } 23 | virtual ~IOPort(){} 24 | virtual size_t send(uint8_t *sendMsg, size_t sendLength) = 0; 25 | virtual size_t recv(uint8_t *recvMsg, size_t recvLength) = 0; 26 | virtual size_t recv(uint8_t *recvMsg) = 0; 27 | void resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs, bool showInfo = true); 28 | bool isDisConnect = false; 29 | protected: 30 | bool _showInfo; 31 | std::string _name; 32 | BlockYN _blockYN = BlockYN::NO; 33 | size_t _recvLength; 34 | timeval _timeout; 35 | timeval _timeoutSaved; 36 | uint16_t _isDisConnectCnt; 37 | }; 38 | 39 | 40 | class UDPPort : public IOPort{ 41 | public: 42 | UDPPort(std::string name, std::string toIP, uint toPort, uint ownPort, 43 | size_t recvLength = 0, 44 | BlockYN blockYN = BlockYN::NO, 45 | size_t timeOutUs = 20000, 46 | bool showInfo = true); 47 | ~UDPPort(); 48 | size_t send(uint8_t *sendMsg, size_t sendMsgLength); 49 | size_t recv(uint8_t *recvMsg, size_t recvLength); 50 | size_t recv(uint8_t *recvMsg); 51 | private: 52 | size_t _recvBlock(uint8_t *recvMsg, size_t recvLength); 53 | size_t _recvUnBlock(uint8_t *recvMsg, size_t recvLength); 54 | sockaddr_in _ownAddr, _toAddr, _fromAddr; 55 | socklen_t _sockaddrSize; 56 | int _sockfd; 57 | int _on = 1; 58 | size_t _sentLength; 59 | 60 | uint8_t _sendBytes[238]; // 7 motors 61 | uint8_t _recvBytes[546]; // 7 motors 62 | 63 | fd_set _rSet; 64 | }; 65 | 66 | 67 | 68 | #endif -------------------------------------------------------------------------------- /include/model/ArmModel.h: -------------------------------------------------------------------------------- 1 | #ifndef ARMMODEL_H 2 | #define ARMMODEL_H 3 | 4 | #include 5 | #include "common/math/robotics.h" 6 | #include "thirdparty/quadProgpp/QuadProg++.hh" 7 | 8 | using namespace robo; 9 | 10 | class ArmModel{ 11 | public: 12 | ArmModel(Vec3 endPosLocal, double endEffectorMass, Vec3 endEffectorCom, Mat3 endEffectorInertia); 13 | ~ArmModel(){}; 14 | 15 | HomoMat forwardKinematics(Vec6 q, int index = 6); 16 | virtual bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false); 17 | Mat6 CalcJacobianSpace(Vec6 q); 18 | Mat6 CalcJacobianBody(Vec6 q); 19 | Vec6 inverseDynamics(Vec6 q, Vec6 qd, Vec6 qdd, Vec6 Ftip); 20 | virtual void solveQP(Vec6 twist, Vec6 qPast, Vec6& qd_result, double dt) = 0; 21 | 22 | virtual bool checkInSingularity(Vec6 q) = 0; 23 | void jointProtect(Vec6& q, Vec6& qd); 24 | 25 | std::vector getJointQMax() {return _jointQMax;} 26 | std::vector getJointQMin() {return _jointQMin;} 27 | std::vector getJointSpeedMax() {return _jointSpeedMax;} 28 | void addLoad(double load); 29 | bool checkAngle(Vec6 ); 30 | 31 | const size_t dof = 6; 32 | protected: 33 | void _buildModel(); 34 | // initial parameters 35 | HomoMat _M; //End posture at the home position 36 | std::vector _Mlist;// List of link frames {i} relative to {i-1} at the home position 37 | Vec3 _gravity; 38 | Mat6 _Slist;// spatial twist at home position 39 | std::vector _Glist; 40 | std::vector _jointAxis;// omega 41 | std::vector _jointPos;// p_0 42 | std::vector _jointQMax; 43 | std::vector _jointQMin; 44 | std::vector _jointSpeedMax; 45 | Vec3 _endPosLocal; // based on mount postion 46 | Vec3 _endMountPosGlobal; 47 | 48 | std::vector _linkPosLocal; 49 | std::vector _disJoint;//distance between joint 50 | std::vector _linkMass; 51 | std::vector _linkCom; // center of mass 52 | std::vector _linkInertia; 53 | double _endEffectorMass; 54 | Vec3 _endEffectorCom; 55 | Mat3 _endEffectorInertia; 56 | 57 | double _load; 58 | }; 59 | 60 | class Z1Model : public ArmModel{ 61 | public: 62 | Z1Model(size_t armType = 36, Vec3 endPosLocal = Vec3::Zero(), double endEffectorMass = 0.0, 63 | Vec3 endEffectorCom = Vec3::Zero(), Mat3 endEffectorInertia = Mat3::Zero()); 64 | ~Z1Model(){}; 65 | 66 | bool checkInSingularity(Vec6 q); 67 | bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false); 68 | void solveQP(Vec6 twist, Vec6 qPast, Vec6& qd_result, double dt); 69 | double _theta2Bias; 70 | private: 71 | void setParam_V3_6(); 72 | void setParam_V3_7(); 73 | void setParam_V3_8(); 74 | }; 75 | 76 | #endif -------------------------------------------------------------------------------- /include/model/unitree_gripper.h: -------------------------------------------------------------------------------- 1 | #ifndef _UNITREE_GRIPPER_H 2 | #define _UNITREE_GRIPPER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "common/math/mathTools.h" 8 | 9 | typedef struct 10 | { 11 | double angle; // Current gripper opening angle. Unit: rad 12 | double speed; // Current measured gripper speed. Unit: rad/s 13 | double tau; // Current measured gripper output torque. Unit: Nm 14 | bool is_grasped; // Indicates whether an object is currently grasped. 15 | int8_t temperature; // current temperature (temperature conduction is slow that leads to lag) 16 | }GripperState; 17 | 18 | typedef struct 19 | { 20 | double angle; // Size of object to grasp in radian 21 | double speed; // Closing speed in rad/s 22 | double tau; // Grasping tau in Nm 23 | double epsilon_inner; // Maximum tolerated deviation when the actual grasped angle is smaller than the commanded grasp angle. 24 | double epsilon_outer; // Maximum tolerated deviation when the actual grasped angle is larger than the commanded grasp angle. 25 | }GripperCmd; 26 | 27 | class Unitree_Gripper 28 | { 29 | public: 30 | Unitree_Gripper(); 31 | Unitree_Gripper(double max_toque); 32 | ~Unitree_Gripper() = default; 33 | 34 | /** 35 | * @brief Move the gripper to target angle 36 | * 37 | * @param angle Target opening angle in rad 38 | * @param speed Opening speed in rad/s 39 | */ 40 | void move(double angle, double speed); 41 | 42 | /** 43 | * @brief Grasps an object 44 | * 45 | * @param angle Size of object to grasp in radian. 46 | * @param speed Closing speed in rad/s. 47 | * @param tau Grasping torque in Nm 48 | * @param epsilon_inner Maximum tolerated deviation when the actual grasped angle 49 | * is smaller than the commanded grasp angle. 50 | * @param epsilon_outer Maximum tolerated deviation when the actual grasped angle 51 | * is larger than the commanded grasp angle. 52 | * @return True if an object has been grasped. 53 | */ 54 | void grasp( double angle, 55 | double speed, 56 | double tau, 57 | double epsilon_inner = 0.01, 58 | double epsilon_outer = 0.01); 59 | 60 | /** 61 | * @brief Get current output torque due to current state 62 | * 63 | * @param angle Measured gripper angle 64 | * @param speed Measured gripper speed 65 | * @param torque Measured gripper torque 66 | * @param temperature Measured gripper temperature 67 | * @return double Output torque 68 | */ 69 | double update(double angle, double speed, double torque, int8_t temperature = 0); 70 | 71 | GripperCmd cmd{}; 72 | GripperState state{}; 73 | private: 74 | enum class GripperCtrlMode{ 75 | Move,// Position 76 | Grasp// Torque 77 | } mode = GripperCtrlMode::Move; 78 | 79 | struct 80 | { 81 | double kp_p; 82 | double kd_p; 83 | double kp_v; 84 | double ki_v; 85 | double kd_v; 86 | } coe{}; 87 | 88 | typedef struct 89 | { 90 | double angle; 91 | double speed; 92 | } Error_state; 93 | 94 | Error_state error{}, error_last{}; 95 | double error_speed_sum_{}; 96 | 97 | static constexpr double MAX_POSITION = -M_PI/2; 98 | static constexpr double MAX_SPEED = M_PI; 99 | double MAX_TORQUE = 20.0; 100 | 101 | std::chrono::steady_clock::time_point first_grasped_time_{}; 102 | bool is_stopped_{}; 103 | double tau_output{}; 104 | 105 | void modify_cmd(GripperCmd& gripper_cmd); 106 | bool is_grasped(); 107 | }; 108 | 109 | #endif // _UNITREE_GRIPPER_H -------------------------------------------------------------------------------- /include/thirdparty/quadProgpp/QuadProg++.hh: -------------------------------------------------------------------------------- 1 | /* 2 | File $Id: QuadProg++.hh 232 2007-06-21 12:29:00Z digasper $ 3 | 4 | The quadprog_solve() function implements the algorithm of Goldfarb and Idnani 5 | for the solution of a (convex) Quadratic Programming problem 6 | by means of an active-set dual method. 7 | 8 | The problem is in the form: 9 | 10 | min 0.5 * x G x + g0 x 11 | s.t. 12 | CE^T x + ce0 = 0 13 | CI^T x + ci0 >= 0 14 | 15 | The matrix and vectors dimensions are as follows: 16 | G: n * n 17 | g0: n 18 | 19 | CE: n * p 20 | ce0: p 21 | 22 | CI: n * m 23 | ci0: m 24 | 25 | x: n 26 | 27 | The function will return the cost of the solution written in the x vector or 28 | std::numeric_limits::infinity() if the problem is infeasible. In the latter case 29 | the value of the x vector is not correct. 30 | 31 | References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving 32 | strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33. 33 | 34 | Notes: 35 | 1. pay attention in setting up the vectors ce0 and ci0. 36 | If the constraints of your problem are specified in the form 37 | A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d. 38 | 2. The matrices have column dimension equal to MATRIX_DIM, 39 | a constant set to 20 in this file (by means of a #define macro). 40 | If the matrices are bigger than 20 x 20 the limit could be 41 | increased by means of a -DMATRIX_DIM=n on the compiler command line. 42 | 3. The matrix G is modified within the function since it is used to compute 43 | the G = L^T L cholesky factorization for further computations inside the function. 44 | If you need the original matrix G you should make a copy of it and pass the copy 45 | to the function. 46 | 47 | Author: Luca Di Gaspero 48 | DIEGM - University of Udine, Italy 49 | luca.digaspero@uniud.it 50 | http://www.diegm.uniud.it/digaspero/ 51 | 52 | The author will be grateful if the researchers using this software will 53 | acknowledge the contribution of this function in their research papers. 54 | 55 | Copyright (c) 2007-2016 Luca Di Gaspero 56 | 57 | This software may be modified and distributed under the terms 58 | of the MIT license. See the LICENSE file for details. 59 | */ 60 | 61 | 62 | #ifndef _QUADPROGPP 63 | #define _QUADPROGPP 64 | 65 | #include "thirdparty/quadProgpp/Array.hh" 66 | #include 67 | 68 | namespace quadprogpp { 69 | 70 | double solve_quadprog(Matrix& G, Vector& g0, 71 | const Matrix& CE, const Vector& ce0, 72 | const Matrix& CI, const Vector& ci0, 73 | Vector& x); 74 | 75 | } // namespace quadprogpp 76 | 77 | #endif // #define _QUADPROGPP 78 | -------------------------------------------------------------------------------- /include/thirdparty/tinyxml/tinystr.h: -------------------------------------------------------------------------------- 1 | /* 2 | www.sourceforge.net/projects/tinyxml 3 | 4 | This software is provided 'as-is', without any express or implied 5 | warranty. In no event will the authors be held liable for any 6 | damages arising from the use of this software. 7 | 8 | Permission is granted to anyone to use this software for any 9 | purpose, including commercial applications, and to alter it and 10 | redistribute it freely, subject to the following restrictions: 11 | 12 | 1. The origin of this software must not be misrepresented; you must 13 | not claim that you wrote the original software. If you use this 14 | software in a product, an acknowledgment in the product documentation 15 | would be appreciated but is not required. 16 | 17 | 2. Altered source versions must be plainly marked as such, and 18 | must not be misrepresented as being the original software. 19 | 20 | 3. This notice may not be removed or altered from any source 21 | distribution. 22 | */ 23 | 24 | 25 | #ifndef TIXML_USE_STL 26 | 27 | #ifndef TIXML_STRING_INCLUDED 28 | #define TIXML_STRING_INCLUDED 29 | 30 | #include 31 | #include 32 | 33 | /* The support for explicit isn't that universal, and it isn't really 34 | required - it is used to check that the TiXmlString class isn't incorrectly 35 | used. Be nice to old compilers and macro it here: 36 | */ 37 | #if defined(_MSC_VER) && (_MSC_VER >= 1200 ) 38 | // Microsoft visual studio, version 6 and higher. 39 | #define TIXML_EXPLICIT explicit 40 | #elif defined(__GNUC__) && (__GNUC__ >= 3 ) 41 | // GCC version 3 and higher.s 42 | #define TIXML_EXPLICIT explicit 43 | #else 44 | #define TIXML_EXPLICIT 45 | #endif 46 | 47 | 48 | /* 49 | TiXmlString is an emulation of a subset of the std::string template. 50 | Its purpose is to allow compiling TinyXML on compilers with no or poor STL support. 51 | Only the member functions relevant to the TinyXML project have been implemented. 52 | The buffer allocation is made by a simplistic power of 2 like mechanism : if we increase 53 | a string and there's no more room, we allocate a buffer twice as big as we need. 54 | */ 55 | class TiXmlString 56 | { 57 | public : 58 | // The size type used 59 | typedef size_t size_type; 60 | 61 | // Error value for find primitive 62 | static const size_type npos; // = -1; 63 | 64 | 65 | // TiXmlString empty constructor 66 | TiXmlString () : rep_(&nullrep_) 67 | { 68 | } 69 | 70 | // TiXmlString copy constructor 71 | TiXmlString ( const TiXmlString & copy) : rep_(0) 72 | { 73 | init(copy.length()); 74 | memcpy(start(), copy.data(), length()); 75 | } 76 | 77 | // TiXmlString constructor, based on a string 78 | TIXML_EXPLICIT TiXmlString ( const char * copy) : rep_(0) 79 | { 80 | init( static_cast( strlen(copy) )); 81 | memcpy(start(), copy, length()); 82 | } 83 | 84 | // TiXmlString constructor, based on a string 85 | TIXML_EXPLICIT TiXmlString ( const char * str, size_type len) : rep_(0) 86 | { 87 | init(len); 88 | memcpy(start(), str, len); 89 | } 90 | 91 | // TiXmlString destructor 92 | ~TiXmlString () 93 | { 94 | quit(); 95 | } 96 | 97 | TiXmlString& operator = (const char * copy) 98 | { 99 | return assign( copy, (size_type)strlen(copy)); 100 | } 101 | 102 | TiXmlString& operator = (const TiXmlString & copy) 103 | { 104 | return assign(copy.start(), copy.length()); 105 | } 106 | 107 | 108 | // += operator. Maps to append 109 | TiXmlString& operator += (const char * suffix) 110 | { 111 | return append(suffix, static_cast( strlen(suffix) )); 112 | } 113 | 114 | // += operator. Maps to append 115 | TiXmlString& operator += (char single) 116 | { 117 | return append(&single, 1); 118 | } 119 | 120 | // += operator. Maps to append 121 | TiXmlString& operator += (const TiXmlString & suffix) 122 | { 123 | return append(suffix.data(), suffix.length()); 124 | } 125 | 126 | 127 | // Convert a TiXmlString into a null-terminated char * 128 | const char * c_str () const { return rep_->str; } 129 | 130 | // Convert a TiXmlString into a char * (need not be null terminated). 131 | const char * data () const { return rep_->str; } 132 | 133 | // Return the length of a TiXmlString 134 | size_type length () const { return rep_->size; } 135 | 136 | // Alias for length() 137 | size_type size () const { return rep_->size; } 138 | 139 | // Checks if a TiXmlString is empty 140 | bool empty () const { return rep_->size == 0; } 141 | 142 | // Return capacity of string 143 | size_type capacity () const { return rep_->capacity; } 144 | 145 | 146 | // single char extraction 147 | const char& at (size_type index) const 148 | { 149 | assert( index < length() ); 150 | return rep_->str[ index ]; 151 | } 152 | 153 | // [] operator 154 | char& operator [] (size_type index) const 155 | { 156 | assert( index < length() ); 157 | return rep_->str[ index ]; 158 | } 159 | 160 | // find a char in a string. Return TiXmlString::npos if not found 161 | size_type find (char lookup) const 162 | { 163 | return find(lookup, 0); 164 | } 165 | 166 | // find a char in a string from an offset. Return TiXmlString::npos if not found 167 | size_type find (char tofind, size_type offset) const 168 | { 169 | if (offset >= length()) return npos; 170 | 171 | for (const char* p = c_str() + offset; *p != '\0'; ++p) 172 | { 173 | if (*p == tofind) return static_cast< size_type >( p - c_str() ); 174 | } 175 | return npos; 176 | } 177 | 178 | void clear () 179 | { 180 | //Lee: 181 | //The original was just too strange, though correct: 182 | // TiXmlString().swap(*this); 183 | //Instead use the quit & re-init: 184 | quit(); 185 | init(0,0); 186 | } 187 | 188 | /* Function to reserve a big amount of data when we know we'll need it. Be aware that this 189 | function DOES NOT clear the content of the TiXmlString if any exists. 190 | */ 191 | void reserve (size_type cap); 192 | 193 | TiXmlString& assign (const char* str, size_type len); 194 | 195 | TiXmlString& append (const char* str, size_type len); 196 | 197 | void swap (TiXmlString& other) 198 | { 199 | Rep* r = rep_; 200 | rep_ = other.rep_; 201 | other.rep_ = r; 202 | } 203 | 204 | private: 205 | 206 | void init(size_type sz) { init(sz, sz); } 207 | void set_size(size_type sz) { rep_->str[ rep_->size = sz ] = '\0'; } 208 | char* start() const { return rep_->str; } 209 | char* finish() const { return rep_->str + rep_->size; } 210 | 211 | struct Rep 212 | { 213 | size_type size, capacity; 214 | char str[1]; 215 | }; 216 | 217 | void init(size_type sz, size_type cap) 218 | { 219 | if (cap) 220 | { 221 | // Lee: the original form: 222 | // rep_ = static_cast(operator new(sizeof(Rep) + cap)); 223 | // doesn't work in some cases of new being overloaded. Switching 224 | // to the normal allocation, although use an 'int' for systems 225 | // that are overly picky about structure alignment. 226 | const size_type bytesNeeded = sizeof(Rep) + cap; 227 | const size_type intsNeeded = ( bytesNeeded + sizeof(int) - 1 ) / sizeof( int ); 228 | rep_ = reinterpret_cast( new int[ intsNeeded ] ); 229 | 230 | rep_->str[ rep_->size = sz ] = '\0'; 231 | rep_->capacity = cap; 232 | } 233 | else 234 | { 235 | rep_ = &nullrep_; 236 | } 237 | } 238 | 239 | void quit() 240 | { 241 | if (rep_ != &nullrep_) 242 | { 243 | // The rep_ is really an array of ints. (see the allocator, above). 244 | // Cast it back before delete, so the compiler won't incorrectly call destructors. 245 | delete [] ( reinterpret_cast( rep_ ) ); 246 | } 247 | } 248 | 249 | Rep * rep_; 250 | static Rep nullrep_; 251 | 252 | } ; 253 | 254 | 255 | inline bool operator == (const TiXmlString & a, const TiXmlString & b) 256 | { 257 | return ( a.length() == b.length() ) // optimization on some platforms 258 | && ( strcmp(a.c_str(), b.c_str()) == 0 ); // actual compare 259 | } 260 | inline bool operator < (const TiXmlString & a, const TiXmlString & b) 261 | { 262 | return strcmp(a.c_str(), b.c_str()) < 0; 263 | } 264 | 265 | inline bool operator != (const TiXmlString & a, const TiXmlString & b) { return !(a == b); } 266 | inline bool operator > (const TiXmlString & a, const TiXmlString & b) { return b < a; } 267 | inline bool operator <= (const TiXmlString & a, const TiXmlString & b) { return !(b < a); } 268 | inline bool operator >= (const TiXmlString & a, const TiXmlString & b) { return !(a < b); } 269 | 270 | inline bool operator == (const TiXmlString & a, const char* b) { return strcmp(a.c_str(), b) == 0; } 271 | inline bool operator == (const char* a, const TiXmlString & b) { return b == a; } 272 | inline bool operator != (const TiXmlString & a, const char* b) { return !(a == b); } 273 | inline bool operator != (const char* a, const TiXmlString & b) { return !(b == a); } 274 | 275 | TiXmlString operator + (const TiXmlString & a, const TiXmlString & b); 276 | TiXmlString operator + (const TiXmlString & a, const char* b); 277 | TiXmlString operator + (const char* a, const TiXmlString & b); 278 | 279 | 280 | /* 281 | TiXmlOutStream is an emulation of std::ostream. It is based on TiXmlString. 282 | Only the operators that we need for TinyXML have been developped. 283 | */ 284 | class TiXmlOutStream : public TiXmlString 285 | { 286 | public : 287 | 288 | // TiXmlOutStream << operator. 289 | TiXmlOutStream & operator << (const TiXmlString & in) 290 | { 291 | *this += in; 292 | return *this; 293 | } 294 | 295 | // TiXmlOutStream << operator. 296 | TiXmlOutStream & operator << (const char * in) 297 | { 298 | *this += in; 299 | return *this; 300 | } 301 | 302 | } ; 303 | 304 | #endif // TIXML_STRING_INCLUDED 305 | #endif // TIXML_USE_STL 306 | -------------------------------------------------------------------------------- /include/thirdparty/tinyxml/tinyxml.h: -------------------------------------------------------------------------------- 1 | /* 2 | www.sourceforge.net/projects/tinyxml 3 | Original code by Lee Thomason (www.grinninglizard.com) 4 | 5 | This software is provided 'as-is', without any express or implied 6 | warranty. In no event will the authors be held liable for any 7 | damages arising from the use of this software. 8 | 9 | Permission is granted to anyone to use this software for any 10 | purpose, including commercial applications, and to alter it and 11 | redistribute it freely, subject to the following restrictions: 12 | 13 | 1. The origin of this software must not be misrepresented; you must 14 | not claim that you wrote the original software. If you use this 15 | software in a product, an acknowledgment in the product documentation 16 | would be appreciated but is not required. 17 | 18 | 2. Altered source versions must be plainly marked as such, and 19 | must not be misrepresented as being the original software. 20 | 21 | 3. This notice may not be removed or altered from any source 22 | distribution. 23 | */ 24 | 25 | 26 | #ifndef TINYXML_INCLUDED 27 | #define TINYXML_INCLUDED 28 | 29 | #ifdef _MSC_VER 30 | #pragma warning( push ) 31 | #pragma warning( disable : 4530 ) 32 | #pragma warning( disable : 4786 ) 33 | #endif 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | // Help out windows: 42 | #if defined( _DEBUG ) && !defined( DEBUG ) 43 | #define DEBUG 44 | #endif 45 | 46 | #ifdef TIXML_USE_STL 47 | #include 48 | #include 49 | #include 50 | #define TIXML_STRING std::string 51 | #else 52 | #include "tinystr.h" 53 | #define TIXML_STRING TiXmlString 54 | #endif 55 | 56 | // Deprecated library function hell. Compilers want to use the 57 | // new safe versions. This probably doesn't fully address the problem, 58 | // but it gets closer. There are too many compilers for me to fully 59 | // test. If you get compilation troubles, undefine TIXML_SAFE 60 | #define TIXML_SAFE 61 | 62 | #ifdef TIXML_SAFE 63 | #if defined(_MSC_VER) && (_MSC_VER >= 1400 ) 64 | // Microsoft visual studio, version 2005 and higher. 65 | #define TIXML_SNPRINTF _snprintf_s 66 | #define TIXML_SSCANF sscanf_s 67 | #elif defined(_MSC_VER) && (_MSC_VER >= 1200 ) 68 | // Microsoft visual studio, version 6 and higher. 69 | //#pragma message( "Using _sn* functions." ) 70 | #define TIXML_SNPRINTF _snprintf 71 | #define TIXML_SSCANF sscanf 72 | #elif defined(__GNUC__) && (__GNUC__ >= 3 ) 73 | // GCC version 3 and higher.s 74 | //#warning( "Using sn* functions." ) 75 | #define TIXML_SNPRINTF snprintf 76 | #define TIXML_SSCANF sscanf 77 | #else 78 | #define TIXML_SNPRINTF snprintf 79 | #define TIXML_SSCANF sscanf 80 | #endif 81 | #endif 82 | 83 | class TiXmlDocument; 84 | class TiXmlElement; 85 | class TiXmlComment; 86 | class TiXmlUnknown; 87 | class TiXmlAttribute; 88 | class TiXmlText; 89 | class TiXmlDeclaration; 90 | class TiXmlParsingData; 91 | 92 | const int TIXML_MAJOR_VERSION = 2; 93 | const int TIXML_MINOR_VERSION = 6; 94 | const int TIXML_PATCH_VERSION = 2; 95 | 96 | /* Internal structure for tracking location of items 97 | in the XML file. 98 | */ 99 | struct TiXmlCursor 100 | { 101 | TiXmlCursor() { Clear(); } 102 | void Clear() { row = col = -1; } 103 | 104 | int row; // 0 based. 105 | int col; // 0 based. 106 | }; 107 | 108 | 109 | /** 110 | Implements the interface to the "Visitor pattern" (see the Accept() method.) 111 | If you call the Accept() method, it requires being passed a TiXmlVisitor 112 | class to handle callbacks. For nodes that contain other nodes (Document, Element) 113 | you will get called with a VisitEnter/VisitExit pair. Nodes that are always leaves 114 | are simply called with Visit(). 115 | 116 | If you return 'true' from a Visit method, recursive parsing will continue. If you return 117 | false, no children of this node or its sibilings will be Visited. 118 | 119 | All flavors of Visit methods have a default implementation that returns 'true' (continue 120 | visiting). You need to only override methods that are interesting to you. 121 | 122 | Generally Accept() is called on the TiXmlDocument, although all nodes suppert Visiting. 123 | 124 | You should never change the document from a callback. 125 | 126 | @sa TiXmlNode::Accept() 127 | */ 128 | class TiXmlVisitor 129 | { 130 | public: 131 | virtual ~TiXmlVisitor() {} 132 | 133 | /// Visit a document. 134 | virtual bool VisitEnter( const TiXmlDocument& /*doc*/ ) { return true; } 135 | /// Visit a document. 136 | virtual bool VisitExit( const TiXmlDocument& /*doc*/ ) { return true; } 137 | 138 | /// Visit an element. 139 | virtual bool VisitEnter( const TiXmlElement& /*element*/, const TiXmlAttribute* /*firstAttribute*/ ) { return true; } 140 | /// Visit an element. 141 | virtual bool VisitExit( const TiXmlElement& /*element*/ ) { return true; } 142 | 143 | /// Visit a declaration 144 | virtual bool Visit( const TiXmlDeclaration& /*declaration*/ ) { return true; } 145 | /// Visit a text node 146 | virtual bool Visit( const TiXmlText& /*text*/ ) { return true; } 147 | /// Visit a comment node 148 | virtual bool Visit( const TiXmlComment& /*comment*/ ) { return true; } 149 | /// Visit an unknown node 150 | virtual bool Visit( const TiXmlUnknown& /*unknown*/ ) { return true; } 151 | }; 152 | 153 | // Only used by Attribute::Query functions 154 | enum 155 | { 156 | TIXML_SUCCESS, 157 | TIXML_NO_ATTRIBUTE, 158 | TIXML_WRONG_TYPE 159 | }; 160 | 161 | 162 | // Used by the parsing routines. 163 | enum TiXmlEncoding 164 | { 165 | TIXML_ENCODING_UNKNOWN, 166 | TIXML_ENCODING_UTF8, 167 | TIXML_ENCODING_LEGACY 168 | }; 169 | 170 | const TiXmlEncoding TIXML_DEFAULT_ENCODING = TIXML_ENCODING_UNKNOWN; 171 | 172 | /** TiXmlBase is a base class for every class in TinyXml. 173 | It does little except to establish that TinyXml classes 174 | can be printed and provide some utility functions. 175 | 176 | In XML, the document and elements can contain 177 | other elements and other types of nodes. 178 | 179 | @verbatim 180 | A Document can contain: Element (container or leaf) 181 | Comment (leaf) 182 | Unknown (leaf) 183 | Declaration( leaf ) 184 | 185 | An Element can contain: Element (container or leaf) 186 | Text (leaf) 187 | Attributes (not on tree) 188 | Comment (leaf) 189 | Unknown (leaf) 190 | 191 | A Decleration contains: Attributes (not on tree) 192 | @endverbatim 193 | */ 194 | class TiXmlBase 195 | { 196 | friend class TiXmlNode; 197 | friend class TiXmlElement; 198 | friend class TiXmlDocument; 199 | 200 | public: 201 | TiXmlBase() : userData(0) {} 202 | virtual ~TiXmlBase() {} 203 | 204 | /** All TinyXml classes can print themselves to a filestream 205 | or the string class (TiXmlString in non-STL mode, std::string 206 | in STL mode.) Either or both cfile and str can be null. 207 | 208 | This is a formatted print, and will insert 209 | tabs and newlines. 210 | 211 | (For an unformatted stream, use the << operator.) 212 | */ 213 | virtual void Print( FILE* cfile, int depth ) const = 0; 214 | 215 | /** The world does not agree on whether white space should be kept or 216 | not. In order to make everyone happy, these global, static functions 217 | are provided to set whether or not TinyXml will condense all white space 218 | into a single space or not. The default is to condense. Note changing this 219 | value is not thread safe. 220 | */ 221 | static void SetCondenseWhiteSpace( bool condense ) { condenseWhiteSpace = condense; } 222 | 223 | /// Return the current white space setting. 224 | static bool IsWhiteSpaceCondensed() { return condenseWhiteSpace; } 225 | 226 | /** Return the position, in the original source file, of this node or attribute. 227 | The row and column are 1-based. (That is the first row and first column is 228 | 1,1). If the returns values are 0 or less, then the parser does not have 229 | a row and column value. 230 | 231 | Generally, the row and column value will be set when the TiXmlDocument::Load(), 232 | TiXmlDocument::LoadFile(), or any TiXmlNode::Parse() is called. It will NOT be set 233 | when the DOM was created from operator>>. 234 | 235 | The values reflect the initial load. Once the DOM is modified programmatically 236 | (by adding or changing nodes and attributes) the new values will NOT update to 237 | reflect changes in the document. 238 | 239 | There is a minor performance cost to computing the row and column. Computation 240 | can be disabled if TiXmlDocument::SetTabSize() is called with 0 as the value. 241 | 242 | @sa TiXmlDocument::SetTabSize() 243 | */ 244 | int Row() const { return location.row + 1; } 245 | int Column() const { return location.col + 1; } ///< See Row() 246 | 247 | void SetUserData( void* user ) { userData = user; } ///< Set a pointer to arbitrary user data. 248 | void* GetUserData() { return userData; } ///< Get a pointer to arbitrary user data. 249 | const void* GetUserData() const { return userData; } ///< Get a pointer to arbitrary user data. 250 | 251 | // Table that returs, for a given lead byte, the total number of bytes 252 | // in the UTF-8 sequence. 253 | static const int utf8ByteTable[256]; 254 | 255 | virtual const char* Parse( const char* p, 256 | TiXmlParsingData* data, 257 | TiXmlEncoding encoding /*= TIXML_ENCODING_UNKNOWN */ ) = 0; 258 | 259 | /** Expands entities in a string. Note this should not contian the tag's '<', '>', etc, 260 | or they will be transformed into entities! 261 | */ 262 | static void EncodeString( const TIXML_STRING& str, TIXML_STRING* out ); 263 | 264 | enum 265 | { 266 | TIXML_NO_ERROR = 0, 267 | TIXML_ERROR, 268 | TIXML_ERROR_OPENING_FILE, 269 | TIXML_ERROR_PARSING_ELEMENT, 270 | TIXML_ERROR_FAILED_TO_READ_ELEMENT_NAME, 271 | TIXML_ERROR_READING_ELEMENT_VALUE, 272 | TIXML_ERROR_READING_ATTRIBUTES, 273 | TIXML_ERROR_PARSING_EMPTY, 274 | TIXML_ERROR_READING_END_TAG, 275 | TIXML_ERROR_PARSING_UNKNOWN, 276 | TIXML_ERROR_PARSING_COMMENT, 277 | TIXML_ERROR_PARSING_DECLARATION, 278 | TIXML_ERROR_DOCUMENT_EMPTY, 279 | TIXML_ERROR_EMBEDDED_NULL, 280 | TIXML_ERROR_PARSING_CDATA, 281 | TIXML_ERROR_DOCUMENT_TOP_ONLY, 282 | 283 | TIXML_ERROR_STRING_COUNT 284 | }; 285 | 286 | protected: 287 | 288 | static const char* SkipWhiteSpace( const char*, TiXmlEncoding encoding ); 289 | 290 | inline static bool IsWhiteSpace( char c ) 291 | { 292 | return ( isspace( (unsigned char) c ) || c == '\n' || c == '\r' ); 293 | } 294 | inline static bool IsWhiteSpace( int c ) 295 | { 296 | if ( c < 256 ) 297 | return IsWhiteSpace( (char) c ); 298 | return false; // Again, only truly correct for English/Latin...but usually works. 299 | } 300 | 301 | #ifdef TIXML_USE_STL 302 | static bool StreamWhiteSpace( std::istream * in, TIXML_STRING * tag ); 303 | static bool StreamTo( std::istream * in, int character, TIXML_STRING * tag ); 304 | #endif 305 | 306 | /* Reads an XML name into the string provided. Returns 307 | a pointer just past the last character of the name, 308 | or 0 if the function has an error. 309 | */ 310 | static const char* ReadName( const char* p, TIXML_STRING* name, TiXmlEncoding encoding ); 311 | 312 | /* Reads text. Returns a pointer past the given end tag. 313 | Wickedly complex options, but it keeps the (sensitive) code in one place. 314 | */ 315 | static const char* ReadText( const char* in, // where to start 316 | TIXML_STRING* text, // the string read 317 | bool ignoreWhiteSpace, // whether to keep the white space 318 | const char* endTag, // what ends this text 319 | bool ignoreCase, // whether to ignore case in the end tag 320 | TiXmlEncoding encoding ); // the current encoding 321 | 322 | // If an entity has been found, transform it into a character. 323 | static const char* GetEntity( const char* in, char* value, int* length, TiXmlEncoding encoding ); 324 | 325 | // Get a character, while interpreting entities. 326 | // The length can be from 0 to 4 bytes. 327 | inline static const char* GetChar( const char* p, char* _value, int* length, TiXmlEncoding encoding ) 328 | { 329 | assert( p ); 330 | if ( encoding == TIXML_ENCODING_UTF8 ) 331 | { 332 | *length = utf8ByteTable[ *((const unsigned char*)p) ]; 333 | assert( *length >= 0 && *length < 5 ); 334 | } 335 | else 336 | { 337 | *length = 1; 338 | } 339 | 340 | if ( *length == 1 ) 341 | { 342 | if ( *p == '&' ) 343 | return GetEntity( p, _value, length, encoding ); 344 | *_value = *p; 345 | return p+1; 346 | } 347 | else if ( *length ) 348 | { 349 | //strncpy( _value, p, *length ); // lots of compilers don't like this function (unsafe), 350 | // and the null terminator isn't needed 351 | for( int i=0; p[i] && i<*length; ++i ) { 352 | _value[i] = p[i]; 353 | } 354 | return p + (*length); 355 | } 356 | else 357 | { 358 | // Not valid text. 359 | return 0; 360 | } 361 | } 362 | 363 | // Return true if the next characters in the stream are any of the endTag sequences. 364 | // Ignore case only works for english, and should only be relied on when comparing 365 | // to English words: StringEqual( p, "version", true ) is fine. 366 | static bool StringEqual( const char* p, 367 | const char* endTag, 368 | bool ignoreCase, 369 | TiXmlEncoding encoding ); 370 | 371 | static const char* errorString[ TIXML_ERROR_STRING_COUNT ]; 372 | 373 | TiXmlCursor location; 374 | 375 | /// Field containing a generic user pointer 376 | void* userData; 377 | 378 | // None of these methods are reliable for any language except English. 379 | // Good for approximation, not great for accuracy. 380 | static int IsAlpha( unsigned char anyByte, TiXmlEncoding encoding ); 381 | static int IsAlphaNum( unsigned char anyByte, TiXmlEncoding encoding ); 382 | inline static int ToLower( int v, TiXmlEncoding encoding ) 383 | { 384 | if ( encoding == TIXML_ENCODING_UTF8 ) 385 | { 386 | if ( v < 128 ) return tolower( v ); 387 | return v; 388 | } 389 | else 390 | { 391 | return tolower( v ); 392 | } 393 | } 394 | static void ConvertUTF32ToUTF8( unsigned long input, char* output, int* length ); 395 | 396 | private: 397 | TiXmlBase( const TiXmlBase& ); // not implemented. 398 | void operator=( const TiXmlBase& base ); // not allowed. 399 | 400 | struct Entity 401 | { 402 | const char* str; 403 | unsigned int strLength; 404 | char chr; 405 | }; 406 | enum 407 | { 408 | NUM_ENTITY = 5, 409 | MAX_ENTITY_LENGTH = 6 410 | 411 | }; 412 | static Entity entity[ NUM_ENTITY ]; 413 | static bool condenseWhiteSpace; 414 | }; 415 | 416 | 417 | /** The parent class for everything in the Document Object Model. 418 | (Except for attributes). 419 | Nodes have siblings, a parent, and children. A node can be 420 | in a document, or stand on its own. The type of a TiXmlNode 421 | can be queried, and it can be cast to its more defined type. 422 | */ 423 | class TiXmlNode : public TiXmlBase 424 | { 425 | friend class TiXmlDocument; 426 | friend class TiXmlElement; 427 | 428 | public: 429 | #ifdef TIXML_USE_STL 430 | 431 | /** An input stream operator, for every class. Tolerant of newlines and 432 | formatting, but doesn't expect them. 433 | */ 434 | friend std::istream& operator >> (std::istream& in, TiXmlNode& base); 435 | 436 | /** An output stream operator, for every class. Note that this outputs 437 | without any newlines or formatting, as opposed to Print(), which 438 | includes tabs and new lines. 439 | 440 | The operator<< and operator>> are not completely symmetric. Writing 441 | a node to a stream is very well defined. You'll get a nice stream 442 | of output, without any extra whitespace or newlines. 443 | 444 | But reading is not as well defined. (As it always is.) If you create 445 | a TiXmlElement (for example) and read that from an input stream, 446 | the text needs to define an element or junk will result. This is 447 | true of all input streams, but it's worth keeping in mind. 448 | 449 | A TiXmlDocument will read nodes until it reads a root element, and 450 | all the children of that root element. 451 | */ 452 | friend std::ostream& operator<< (std::ostream& out, const TiXmlNode& base); 453 | 454 | /// Appends the XML node or attribute to a std::string. 455 | friend std::string& operator<< (std::string& out, const TiXmlNode& base ); 456 | 457 | #endif 458 | 459 | /** The types of XML nodes supported by TinyXml. (All the 460 | unsupported types are picked up by UNKNOWN.) 461 | */ 462 | enum NodeType 463 | { 464 | TINYXML_DOCUMENT, 465 | TINYXML_ELEMENT, 466 | TINYXML_COMMENT, 467 | TINYXML_UNKNOWN, 468 | TINYXML_TEXT, 469 | TINYXML_DECLARATION, 470 | TINYXML_TYPECOUNT 471 | }; 472 | 473 | virtual ~TiXmlNode(); 474 | 475 | /** The meaning of 'value' changes for the specific type of 476 | TiXmlNode. 477 | @verbatim 478 | Document: filename of the xml file 479 | Element: name of the element 480 | Comment: the comment text 481 | Unknown: the tag contents 482 | Text: the text string 483 | @endverbatim 484 | 485 | The subclasses will wrap this function. 486 | */ 487 | const char *Value() const { return value.c_str (); } 488 | 489 | #ifdef TIXML_USE_STL 490 | /** Return Value() as a std::string. If you only use STL, 491 | this is more efficient than calling Value(). 492 | Only available in STL mode. 493 | */ 494 | const std::string& ValueStr() const { return value; } 495 | #endif 496 | 497 | const TIXML_STRING& ValueTStr() const { return value; } 498 | 499 | /** Changes the value of the node. Defined as: 500 | @verbatim 501 | Document: filename of the xml file 502 | Element: name of the element 503 | Comment: the comment text 504 | Unknown: the tag contents 505 | Text: the text string 506 | @endverbatim 507 | */ 508 | void SetValue(const char * _value) { value = _value;} 509 | 510 | #ifdef TIXML_USE_STL 511 | /// STL std::string form. 512 | void SetValue( const std::string& _value ) { value = _value; } 513 | #endif 514 | 515 | /// Delete all the children of this node. Does not affect 'this'. 516 | void Clear(); 517 | 518 | /// One step up the DOM. 519 | TiXmlNode* Parent() { return parent; } 520 | const TiXmlNode* Parent() const { return parent; } 521 | 522 | const TiXmlNode* FirstChild() const { return firstChild; } ///< The first child of this node. Will be null if there are no children. 523 | TiXmlNode* FirstChild() { return firstChild; } 524 | const TiXmlNode* FirstChild( const char * value ) const; ///< The first child of this node with the matching 'value'. Will be null if none found. 525 | /// The first child of this node with the matching 'value'. Will be null if none found. 526 | TiXmlNode* FirstChild( const char * _value ) { 527 | // Call through to the const version - safe since nothing is changed. Exiting syntax: cast this to a const (always safe) 528 | // call the method, cast the return back to non-const. 529 | return const_cast< TiXmlNode* > ((const_cast< const TiXmlNode* >(this))->FirstChild( _value )); 530 | } 531 | const TiXmlNode* LastChild() const { return lastChild; } /// The last child of this node. Will be null if there are no children. 532 | TiXmlNode* LastChild() { return lastChild; } 533 | 534 | const TiXmlNode* LastChild( const char * value ) const; /// The last child of this node matching 'value'. Will be null if there are no children. 535 | TiXmlNode* LastChild( const char * _value ) { 536 | return const_cast< TiXmlNode* > ((const_cast< const TiXmlNode* >(this))->LastChild( _value )); 537 | } 538 | 539 | #ifdef TIXML_USE_STL 540 | const TiXmlNode* FirstChild( const std::string& _value ) const { return FirstChild (_value.c_str ()); } ///< STL std::string form. 541 | TiXmlNode* FirstChild( const std::string& _value ) { return FirstChild (_value.c_str ()); } ///< STL std::string form. 542 | const TiXmlNode* LastChild( const std::string& _value ) const { return LastChild (_value.c_str ()); } ///< STL std::string form. 543 | TiXmlNode* LastChild( const std::string& _value ) { return LastChild (_value.c_str ()); } ///< STL std::string form. 544 | #endif 545 | 546 | /** An alternate way to walk the children of a node. 547 | One way to iterate over nodes is: 548 | @verbatim 549 | for( child = parent->FirstChild(); child; child = child->NextSibling() ) 550 | @endverbatim 551 | 552 | IterateChildren does the same thing with the syntax: 553 | @verbatim 554 | child = 0; 555 | while( child = parent->IterateChildren( child ) ) 556 | @endverbatim 557 | 558 | IterateChildren takes the previous child as input and finds 559 | the next one. If the previous child is null, it returns the 560 | first. IterateChildren will return null when done. 561 | */ 562 | const TiXmlNode* IterateChildren( const TiXmlNode* previous ) const; 563 | TiXmlNode* IterateChildren( const TiXmlNode* previous ) { 564 | return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->IterateChildren( previous ) ); 565 | } 566 | 567 | /// This flavor of IterateChildren searches for children with a particular 'value' 568 | const TiXmlNode* IterateChildren( const char * value, const TiXmlNode* previous ) const; 569 | TiXmlNode* IterateChildren( const char * _value, const TiXmlNode* previous ) { 570 | return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->IterateChildren( _value, previous ) ); 571 | } 572 | 573 | #ifdef TIXML_USE_STL 574 | const TiXmlNode* IterateChildren( const std::string& _value, const TiXmlNode* previous ) const { return IterateChildren (_value.c_str (), previous); } ///< STL std::string form. 575 | TiXmlNode* IterateChildren( const std::string& _value, const TiXmlNode* previous ) { return IterateChildren (_value.c_str (), previous); } ///< STL std::string form. 576 | #endif 577 | 578 | /** Add a new node related to this. Adds a child past the LastChild. 579 | Returns a pointer to the new object or NULL if an error occured. 580 | */ 581 | TiXmlNode* InsertEndChild( const TiXmlNode& addThis ); 582 | 583 | 584 | /** Add a new node related to this. Adds a child past the LastChild. 585 | 586 | NOTE: the node to be added is passed by pointer, and will be 587 | henceforth owned (and deleted) by tinyXml. This method is efficient 588 | and avoids an extra copy, but should be used with care as it 589 | uses a different memory model than the other insert functions. 590 | 591 | @sa InsertEndChild 592 | */ 593 | TiXmlNode* LinkEndChild( TiXmlNode* addThis ); 594 | 595 | /** Add a new node related to this. Adds a child before the specified child. 596 | Returns a pointer to the new object or NULL if an error occured. 597 | */ 598 | TiXmlNode* InsertBeforeChild( TiXmlNode* beforeThis, const TiXmlNode& addThis ); 599 | 600 | /** Add a new node related to this. Adds a child after the specified child. 601 | Returns a pointer to the new object or NULL if an error occured. 602 | */ 603 | TiXmlNode* InsertAfterChild( TiXmlNode* afterThis, const TiXmlNode& addThis ); 604 | 605 | /** Replace a child of this node. 606 | Returns a pointer to the new object or NULL if an error occured. 607 | */ 608 | TiXmlNode* ReplaceChild( TiXmlNode* replaceThis, const TiXmlNode& withThis ); 609 | 610 | /// Delete a child of this node. 611 | bool RemoveChild( TiXmlNode* removeThis ); 612 | 613 | /// Navigate to a sibling node. 614 | const TiXmlNode* PreviousSibling() const { return prev; } 615 | TiXmlNode* PreviousSibling() { return prev; } 616 | 617 | /// Navigate to a sibling node. 618 | const TiXmlNode* PreviousSibling( const char * ) const; 619 | TiXmlNode* PreviousSibling( const char *_prev ) { 620 | return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->PreviousSibling( _prev ) ); 621 | } 622 | 623 | #ifdef TIXML_USE_STL 624 | const TiXmlNode* PreviousSibling( const std::string& _value ) const { return PreviousSibling (_value.c_str ()); } ///< STL std::string form. 625 | TiXmlNode* PreviousSibling( const std::string& _value ) { return PreviousSibling (_value.c_str ()); } ///< STL std::string form. 626 | const TiXmlNode* NextSibling( const std::string& _value) const { return NextSibling (_value.c_str ()); } ///< STL std::string form. 627 | TiXmlNode* NextSibling( const std::string& _value) { return NextSibling (_value.c_str ()); } ///< STL std::string form. 628 | #endif 629 | 630 | /// Navigate to a sibling node. 631 | const TiXmlNode* NextSibling() const { return next; } 632 | TiXmlNode* NextSibling() { return next; } 633 | 634 | /// Navigate to a sibling node with the given 'value'. 635 | const TiXmlNode* NextSibling( const char * ) const; 636 | TiXmlNode* NextSibling( const char* _next ) { 637 | return const_cast< TiXmlNode* >( (const_cast< const TiXmlNode* >(this))->NextSibling( _next ) ); 638 | } 639 | 640 | /** Convenience function to get through elements. 641 | Calls NextSibling and ToElement. Will skip all non-Element 642 | nodes. Returns 0 if there is not another element. 643 | */ 644 | const TiXmlElement* NextSiblingElement() const; 645 | TiXmlElement* NextSiblingElement() { 646 | return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->NextSiblingElement() ); 647 | } 648 | 649 | /** Convenience function to get through elements. 650 | Calls NextSibling and ToElement. Will skip all non-Element 651 | nodes. Returns 0 if there is not another element. 652 | */ 653 | const TiXmlElement* NextSiblingElement( const char * ) const; 654 | TiXmlElement* NextSiblingElement( const char *_next ) { 655 | return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->NextSiblingElement( _next ) ); 656 | } 657 | 658 | #ifdef TIXML_USE_STL 659 | const TiXmlElement* NextSiblingElement( const std::string& _value) const { return NextSiblingElement (_value.c_str ()); } ///< STL std::string form. 660 | TiXmlElement* NextSiblingElement( const std::string& _value) { return NextSiblingElement (_value.c_str ()); } ///< STL std::string form. 661 | #endif 662 | 663 | /// Convenience function to get through elements. 664 | const TiXmlElement* FirstChildElement() const; 665 | TiXmlElement* FirstChildElement() { 666 | return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->FirstChildElement() ); 667 | } 668 | 669 | /// Convenience function to get through elements. 670 | const TiXmlElement* FirstChildElement( const char * _value ) const; 671 | TiXmlElement* FirstChildElement( const char * _value ) { 672 | return const_cast< TiXmlElement* >( (const_cast< const TiXmlNode* >(this))->FirstChildElement( _value ) ); 673 | } 674 | 675 | #ifdef TIXML_USE_STL 676 | const TiXmlElement* FirstChildElement( const std::string& _value ) const { return FirstChildElement (_value.c_str ()); } ///< STL std::string form. 677 | TiXmlElement* FirstChildElement( const std::string& _value ) { return FirstChildElement (_value.c_str ()); } ///< STL std::string form. 678 | #endif 679 | 680 | /** Query the type (as an enumerated value, above) of this node. 681 | The possible types are: TINYXML_DOCUMENT, TINYXML_ELEMENT, TINYXML_COMMENT, 682 | TINYXML_UNKNOWN, TINYXML_TEXT, and TINYXML_DECLARATION. 683 | */ 684 | int Type() const { return type; } 685 | 686 | /** Return a pointer to the Document this node lives in. 687 | Returns null if not in a document. 688 | */ 689 | const TiXmlDocument* GetDocument() const; 690 | TiXmlDocument* GetDocument() { 691 | return const_cast< TiXmlDocument* >( (const_cast< const TiXmlNode* >(this))->GetDocument() ); 692 | } 693 | 694 | /// Returns true if this node has no children. 695 | bool NoChildren() const { return !firstChild; } 696 | 697 | virtual const TiXmlDocument* ToDocument() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. 698 | virtual const TiXmlElement* ToElement() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. 699 | virtual const TiXmlComment* ToComment() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. 700 | virtual const TiXmlUnknown* ToUnknown() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. 701 | virtual const TiXmlText* ToText() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. 702 | virtual const TiXmlDeclaration* ToDeclaration() const { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. 703 | 704 | virtual TiXmlDocument* ToDocument() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. 705 | virtual TiXmlElement* ToElement() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. 706 | virtual TiXmlComment* ToComment() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. 707 | virtual TiXmlUnknown* ToUnknown() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. 708 | virtual TiXmlText* ToText() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. 709 | virtual TiXmlDeclaration* ToDeclaration() { return 0; } ///< Cast to a more defined type. Will return null if not of the requested type. 710 | 711 | /** Create an exact duplicate of this node and return it. The memory must be deleted 712 | by the caller. 713 | */ 714 | virtual TiXmlNode* Clone() const = 0; 715 | 716 | /** Accept a hierchical visit the nodes in the TinyXML DOM. Every node in the 717 | XML tree will be conditionally visited and the host will be called back 718 | via the TiXmlVisitor interface. 719 | 720 | This is essentially a SAX interface for TinyXML. (Note however it doesn't re-parse 721 | the XML for the callbacks, so the performance of TinyXML is unchanged by using this 722 | interface versus any other.) 723 | 724 | The interface has been based on ideas from: 725 | 726 | - http://www.saxproject.org/ 727 | - http://c2.com/cgi/wiki?HierarchicalVisitorPattern 728 | 729 | Which are both good references for "visiting". 730 | 731 | An example of using Accept(): 732 | @verbatim 733 | TiXmlPrinter printer; 734 | tinyxmlDoc.Accept( &printer ); 735 | const char* xmlcstr = printer.CStr(); 736 | @endverbatim 737 | */ 738 | virtual bool Accept( TiXmlVisitor* visitor ) const = 0; 739 | 740 | protected: 741 | TiXmlNode( NodeType _type ); 742 | 743 | // Copy to the allocated object. Shared functionality between Clone, Copy constructor, 744 | // and the assignment operator. 745 | void CopyTo( TiXmlNode* target ) const; 746 | 747 | #ifdef TIXML_USE_STL 748 | // The real work of the input operator. 749 | virtual void StreamIn( std::istream* in, TIXML_STRING* tag ) = 0; 750 | #endif 751 | 752 | // Figure out what is at *p, and parse it. Returns null if it is not an xml node. 753 | TiXmlNode* Identify( const char* start, TiXmlEncoding encoding ); 754 | 755 | TiXmlNode* parent; 756 | NodeType type; 757 | 758 | TiXmlNode* firstChild; 759 | TiXmlNode* lastChild; 760 | 761 | TIXML_STRING value; 762 | 763 | TiXmlNode* prev; 764 | TiXmlNode* next; 765 | 766 | private: 767 | TiXmlNode( const TiXmlNode& ); // not implemented. 768 | void operator=( const TiXmlNode& base ); // not allowed. 769 | }; 770 | 771 | 772 | /** An attribute is a name-value pair. Elements have an arbitrary 773 | number of attributes, each with a unique name. 774 | 775 | @note The attributes are not TiXmlNodes, since they are not 776 | part of the tinyXML document object model. There are other 777 | suggested ways to look at this problem. 778 | */ 779 | class TiXmlAttribute : public TiXmlBase 780 | { 781 | friend class TiXmlAttributeSet; 782 | 783 | public: 784 | /// Construct an empty attribute. 785 | TiXmlAttribute() : TiXmlBase() 786 | { 787 | document = 0; 788 | prev = next = 0; 789 | } 790 | 791 | #ifdef TIXML_USE_STL 792 | /// std::string constructor. 793 | TiXmlAttribute( const std::string& _name, const std::string& _value ) 794 | { 795 | name = _name; 796 | value = _value; 797 | document = 0; 798 | prev = next = 0; 799 | } 800 | #endif 801 | 802 | /// Construct an attribute with a name and value. 803 | TiXmlAttribute( const char * _name, const char * _value ) 804 | { 805 | name = _name; 806 | value = _value; 807 | document = 0; 808 | prev = next = 0; 809 | } 810 | 811 | const char* Name() const { return name.c_str(); } ///< Return the name of this attribute. 812 | const char* Value() const { return value.c_str(); } ///< Return the value of this attribute. 813 | #ifdef TIXML_USE_STL 814 | const std::string& ValueStr() const { return value; } ///< Return the value of this attribute. 815 | #endif 816 | int IntValue() const; ///< Return the value of this attribute, converted to an integer. 817 | double DoubleValue() const; ///< Return the value of this attribute, converted to a double. 818 | 819 | // Get the tinyxml string representation 820 | const TIXML_STRING& NameTStr() const { return name; } 821 | 822 | /** QueryIntValue examines the value string. It is an alternative to the 823 | IntValue() method with richer error checking. 824 | If the value is an integer, it is stored in 'value' and 825 | the call returns TIXML_SUCCESS. If it is not 826 | an integer, it returns TIXML_WRONG_TYPE. 827 | 828 | A specialized but useful call. Note that for success it returns 0, 829 | which is the opposite of almost all other TinyXml calls. 830 | */ 831 | int QueryIntValue( int* _value ) const; 832 | /// QueryDoubleValue examines the value string. See QueryIntValue(). 833 | int QueryDoubleValue( double* _value ) const; 834 | 835 | void SetName( const char* _name ) { name = _name; } ///< Set the name of this attribute. 836 | void SetValue( const char* _value ) { value = _value; } ///< Set the value. 837 | 838 | void SetIntValue( int _value ); ///< Set the value from an integer. 839 | void SetDoubleValue( double _value ); ///< Set the value from a double. 840 | 841 | #ifdef TIXML_USE_STL 842 | /// STL std::string form. 843 | void SetName( const std::string& _name ) { name = _name; } 844 | /// STL std::string form. 845 | void SetValue( const std::string& _value ) { value = _value; } 846 | #endif 847 | 848 | /// Get the next sibling attribute in the DOM. Returns null at end. 849 | const TiXmlAttribute* Next() const; 850 | TiXmlAttribute* Next() { 851 | return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttribute* >(this))->Next() ); 852 | } 853 | 854 | /// Get the previous sibling attribute in the DOM. Returns null at beginning. 855 | const TiXmlAttribute* Previous() const; 856 | TiXmlAttribute* Previous() { 857 | return const_cast< TiXmlAttribute* >( (const_cast< const TiXmlAttribute* >(this))->Previous() ); 858 | } 859 | 860 | bool operator==( const TiXmlAttribute& rhs ) const { return rhs.name == name; } 861 | bool operator<( const TiXmlAttribute& rhs ) const { return name < rhs.name; } 862 | bool operator>( const TiXmlAttribute& rhs ) const { return name > rhs.name; } 863 | 864 | /* Attribute parsing starts: first letter of the name 865 | returns: the next char after the value end quote 866 | */ 867 | virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); 868 | 869 | // Prints this Attribute to a FILE stream. 870 | virtual void Print( FILE* cfile, int depth ) const { 871 | Print( cfile, depth, 0 ); 872 | } 873 | void Print( FILE* cfile, int depth, TIXML_STRING* str ) const; 874 | 875 | // [internal use] 876 | // Set the document pointer so the attribute can report errors. 877 | void SetDocument( TiXmlDocument* doc ) { document = doc; } 878 | 879 | private: 880 | TiXmlAttribute( const TiXmlAttribute& ); // not implemented. 881 | void operator=( const TiXmlAttribute& base ); // not allowed. 882 | 883 | TiXmlDocument* document; // A pointer back to a document, for error reporting. 884 | TIXML_STRING name; 885 | TIXML_STRING value; 886 | TiXmlAttribute* prev; 887 | TiXmlAttribute* next; 888 | }; 889 | 890 | 891 | /* A class used to manage a group of attributes. 892 | It is only used internally, both by the ELEMENT and the DECLARATION. 893 | 894 | The set can be changed transparent to the Element and Declaration 895 | classes that use it, but NOT transparent to the Attribute 896 | which has to implement a next() and previous() method. Which makes 897 | it a bit problematic and prevents the use of STL. 898 | 899 | This version is implemented with circular lists because: 900 | - I like circular lists 901 | - it demonstrates some independence from the (typical) doubly linked list. 902 | */ 903 | class TiXmlAttributeSet 904 | { 905 | public: 906 | TiXmlAttributeSet(); 907 | ~TiXmlAttributeSet(); 908 | 909 | void Add( TiXmlAttribute* attribute ); 910 | void Remove( TiXmlAttribute* attribute ); 911 | 912 | const TiXmlAttribute* First() const { return ( sentinel.next == &sentinel ) ? 0 : sentinel.next; } 913 | TiXmlAttribute* First() { return ( sentinel.next == &sentinel ) ? 0 : sentinel.next; } 914 | const TiXmlAttribute* Last() const { return ( sentinel.prev == &sentinel ) ? 0 : sentinel.prev; } 915 | TiXmlAttribute* Last() { return ( sentinel.prev == &sentinel ) ? 0 : sentinel.prev; } 916 | 917 | TiXmlAttribute* Find( const char* _name ) const; 918 | TiXmlAttribute* FindOrCreate( const char* _name ); 919 | 920 | # ifdef TIXML_USE_STL 921 | TiXmlAttribute* Find( const std::string& _name ) const; 922 | TiXmlAttribute* FindOrCreate( const std::string& _name ); 923 | # endif 924 | 925 | 926 | private: 927 | //*ME: Because of hidden/disabled copy-construktor in TiXmlAttribute (sentinel-element), 928 | //*ME: this class must be also use a hidden/disabled copy-constructor !!! 929 | TiXmlAttributeSet( const TiXmlAttributeSet& ); // not allowed 930 | void operator=( const TiXmlAttributeSet& ); // not allowed (as TiXmlAttribute) 931 | 932 | TiXmlAttribute sentinel; 933 | }; 934 | 935 | 936 | /** The element is a container class. It has a value, the element name, 937 | and can contain other elements, text, comments, and unknowns. 938 | Elements also contain an arbitrary number of attributes. 939 | */ 940 | class TiXmlElement : public TiXmlNode 941 | { 942 | public: 943 | /// Construct an element. 944 | TiXmlElement (const char * in_value); 945 | 946 | #ifdef TIXML_USE_STL 947 | /// std::string constructor. 948 | TiXmlElement( const std::string& _value ); 949 | #endif 950 | 951 | TiXmlElement( const TiXmlElement& ); 952 | 953 | TiXmlElement& operator=( const TiXmlElement& base ); 954 | 955 | virtual ~TiXmlElement(); 956 | 957 | /** Given an attribute name, Attribute() returns the value 958 | for the attribute of that name, or null if none exists. 959 | */ 960 | const char* Attribute( const char* name ) const; 961 | 962 | /** Given an attribute name, Attribute() returns the value 963 | for the attribute of that name, or null if none exists. 964 | If the attribute exists and can be converted to an integer, 965 | the integer value will be put in the return 'i', if 'i' 966 | is non-null. 967 | */ 968 | const char* Attribute( const char* name, int* i ) const; 969 | 970 | /** Given an attribute name, Attribute() returns the value 971 | for the attribute of that name, or null if none exists. 972 | If the attribute exists and can be converted to an double, 973 | the double value will be put in the return 'd', if 'd' 974 | is non-null. 975 | */ 976 | const char* Attribute( const char* name, double* d ) const; 977 | 978 | /** QueryIntAttribute examines the attribute - it is an alternative to the 979 | Attribute() method with richer error checking. 980 | If the attribute is an integer, it is stored in 'value' and 981 | the call returns TIXML_SUCCESS. If it is not 982 | an integer, it returns TIXML_WRONG_TYPE. If the attribute 983 | does not exist, then TIXML_NO_ATTRIBUTE is returned. 984 | */ 985 | int QueryIntAttribute( const char* name, int* _value ) const; 986 | /// QueryUnsignedAttribute examines the attribute - see QueryIntAttribute(). 987 | int QueryUnsignedAttribute( const char* name, unsigned* _value ) const; 988 | /** QueryBoolAttribute examines the attribute - see QueryIntAttribute(). 989 | Note that '1', 'true', or 'yes' are considered true, while '0', 'false' 990 | and 'no' are considered false. 991 | */ 992 | int QueryBoolAttribute( const char* name, bool* _value ) const; 993 | /// QueryDoubleAttribute examines the attribute - see QueryIntAttribute(). 994 | int QueryDoubleAttribute( const char* name, double* _value ) const; 995 | /// QueryFloatAttribute examines the attribute - see QueryIntAttribute(). 996 | int QueryFloatAttribute( const char* name, float* _value ) const { 997 | double d; 998 | int result = QueryDoubleAttribute( name, &d ); 999 | if ( result == TIXML_SUCCESS ) { 1000 | *_value = (float)d; 1001 | } 1002 | return result; 1003 | } 1004 | 1005 | #ifdef TIXML_USE_STL 1006 | /// QueryStringAttribute examines the attribute - see QueryIntAttribute(). 1007 | int QueryStringAttribute( const char* name, std::string* _value ) const { 1008 | const char* cstr = Attribute( name ); 1009 | if ( cstr ) { 1010 | *_value = std::string( cstr ); 1011 | return TIXML_SUCCESS; 1012 | } 1013 | return TIXML_NO_ATTRIBUTE; 1014 | } 1015 | 1016 | /** Template form of the attribute query which will try to read the 1017 | attribute into the specified type. Very easy, very powerful, but 1018 | be careful to make sure to call this with the correct type. 1019 | 1020 | NOTE: This method doesn't work correctly for 'string' types that contain spaces. 1021 | 1022 | @return TIXML_SUCCESS, TIXML_WRONG_TYPE, or TIXML_NO_ATTRIBUTE 1023 | */ 1024 | template< typename T > int QueryValueAttribute( const std::string& name, T* outValue ) const 1025 | { 1026 | const TiXmlAttribute* node = attributeSet.Find( name ); 1027 | if ( !node ) 1028 | return TIXML_NO_ATTRIBUTE; 1029 | 1030 | std::stringstream sstream( node->ValueStr() ); 1031 | sstream >> *outValue; 1032 | if ( !sstream.fail() ) 1033 | return TIXML_SUCCESS; 1034 | return TIXML_WRONG_TYPE; 1035 | } 1036 | 1037 | int QueryValueAttribute( const std::string& name, std::string* outValue ) const 1038 | { 1039 | const TiXmlAttribute* node = attributeSet.Find( name ); 1040 | if ( !node ) 1041 | return TIXML_NO_ATTRIBUTE; 1042 | *outValue = node->ValueStr(); 1043 | return TIXML_SUCCESS; 1044 | } 1045 | #endif 1046 | 1047 | /** Sets an attribute of name to a given value. The attribute 1048 | will be created if it does not exist, or changed if it does. 1049 | */ 1050 | void SetAttribute( const char* name, const char * _value ); 1051 | 1052 | #ifdef TIXML_USE_STL 1053 | const std::string* Attribute( const std::string& name ) const; 1054 | const std::string* Attribute( const std::string& name, int* i ) const; 1055 | const std::string* Attribute( const std::string& name, double* d ) const; 1056 | int QueryIntAttribute( const std::string& name, int* _value ) const; 1057 | int QueryDoubleAttribute( const std::string& name, double* _value ) const; 1058 | 1059 | /// STL std::string form. 1060 | void SetAttribute( const std::string& name, const std::string& _value ); 1061 | ///< STL std::string form. 1062 | void SetAttribute( const std::string& name, int _value ); 1063 | ///< STL std::string form. 1064 | void SetDoubleAttribute( const std::string& name, double value ); 1065 | #endif 1066 | 1067 | /** Sets an attribute of name to a given value. The attribute 1068 | will be created if it does not exist, or changed if it does. 1069 | */ 1070 | void SetAttribute( const char * name, int value ); 1071 | 1072 | /** Sets an attribute of name to a given value. The attribute 1073 | will be created if it does not exist, or changed if it does. 1074 | */ 1075 | void SetDoubleAttribute( const char * name, double value ); 1076 | 1077 | /** Deletes an attribute with the given name. 1078 | */ 1079 | void RemoveAttribute( const char * name ); 1080 | #ifdef TIXML_USE_STL 1081 | void RemoveAttribute( const std::string& name ) { RemoveAttribute (name.c_str ()); } ///< STL std::string form. 1082 | #endif 1083 | 1084 | const TiXmlAttribute* FirstAttribute() const { return attributeSet.First(); } ///< Access the first attribute in this element. 1085 | TiXmlAttribute* FirstAttribute() { return attributeSet.First(); } 1086 | const TiXmlAttribute* LastAttribute() const { return attributeSet.Last(); } ///< Access the last attribute in this element. 1087 | TiXmlAttribute* LastAttribute() { return attributeSet.Last(); } 1088 | 1089 | /** Convenience function for easy access to the text inside an element. Although easy 1090 | and concise, GetText() is limited compared to getting the TiXmlText child 1091 | and accessing it directly. 1092 | 1093 | If the first child of 'this' is a TiXmlText, the GetText() 1094 | returns the character string of the Text node, else null is returned. 1095 | 1096 | This is a convenient method for getting the text of simple contained text: 1097 | @verbatim 1098 | This is text 1099 | const char* str = fooElement->GetText(); 1100 | @endverbatim 1101 | 1102 | 'str' will be a pointer to "This is text". 1103 | 1104 | Note that this function can be misleading. If the element foo was created from 1105 | this XML: 1106 | @verbatim 1107 | This is text 1108 | @endverbatim 1109 | 1110 | then the value of str would be null. The first child node isn't a text node, it is 1111 | another element. From this XML: 1112 | @verbatim 1113 | This is text 1114 | @endverbatim 1115 | GetText() will return "This is ". 1116 | 1117 | WARNING: GetText() accesses a child node - don't become confused with the 1118 | similarly named TiXmlHandle::Text() and TiXmlNode::ToText() which are 1119 | safe type casts on the referenced node. 1120 | */ 1121 | const char* GetText() const; 1122 | 1123 | /// Creates a new Element and returns it - the returned element is a copy. 1124 | virtual TiXmlNode* Clone() const; 1125 | // Print the Element to a FILE stream. 1126 | virtual void Print( FILE* cfile, int depth ) const; 1127 | 1128 | /* Attribtue parsing starts: next char past '<' 1129 | returns: next char past '>' 1130 | */ 1131 | virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); 1132 | 1133 | virtual const TiXmlElement* ToElement() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. 1134 | virtual TiXmlElement* ToElement() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. 1135 | 1136 | /** Walk the XML tree visiting this node and all of its children. 1137 | */ 1138 | virtual bool Accept( TiXmlVisitor* visitor ) const; 1139 | 1140 | protected: 1141 | 1142 | void CopyTo( TiXmlElement* target ) const; 1143 | void ClearThis(); // like clear, but initializes 'this' object as well 1144 | 1145 | // Used to be public [internal use] 1146 | #ifdef TIXML_USE_STL 1147 | virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); 1148 | #endif 1149 | /* [internal use] 1150 | Reads the "value" of the element -- another element, or text. 1151 | This should terminate with the current end tag. 1152 | */ 1153 | const char* ReadValue( const char* in, TiXmlParsingData* prevData, TiXmlEncoding encoding ); 1154 | 1155 | private: 1156 | TiXmlAttributeSet attributeSet; 1157 | }; 1158 | 1159 | 1160 | /** An XML comment. 1161 | */ 1162 | class TiXmlComment : public TiXmlNode 1163 | { 1164 | public: 1165 | /// Constructs an empty comment. 1166 | TiXmlComment() : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) {} 1167 | /// Construct a comment from text. 1168 | TiXmlComment( const char* _value ) : TiXmlNode( TiXmlNode::TINYXML_COMMENT ) { 1169 | SetValue( _value ); 1170 | } 1171 | TiXmlComment( const TiXmlComment& ); 1172 | TiXmlComment& operator=( const TiXmlComment& base ); 1173 | 1174 | virtual ~TiXmlComment() {} 1175 | 1176 | /// Returns a copy of this Comment. 1177 | virtual TiXmlNode* Clone() const; 1178 | // Write this Comment to a FILE stream. 1179 | virtual void Print( FILE* cfile, int depth ) const; 1180 | 1181 | /* Attribtue parsing starts: at the ! of the !-- 1182 | returns: next char past '>' 1183 | */ 1184 | virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); 1185 | 1186 | virtual const TiXmlComment* ToComment() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. 1187 | virtual TiXmlComment* ToComment() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. 1188 | 1189 | /** Walk the XML tree visiting this node and all of its children. 1190 | */ 1191 | virtual bool Accept( TiXmlVisitor* visitor ) const; 1192 | 1193 | protected: 1194 | void CopyTo( TiXmlComment* target ) const; 1195 | 1196 | // used to be public 1197 | #ifdef TIXML_USE_STL 1198 | virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); 1199 | #endif 1200 | // virtual void StreamOut( TIXML_OSTREAM * out ) const; 1201 | 1202 | private: 1203 | 1204 | }; 1205 | 1206 | 1207 | /** XML text. A text node can have 2 ways to output the next. "normal" output 1208 | and CDATA. It will default to the mode it was parsed from the XML file and 1209 | you generally want to leave it alone, but you can change the output mode with 1210 | SetCDATA() and query it with CDATA(). 1211 | */ 1212 | class TiXmlText : public TiXmlNode 1213 | { 1214 | friend class TiXmlElement; 1215 | public: 1216 | /** Constructor for text element. By default, it is treated as 1217 | normal, encoded text. If you want it be output as a CDATA text 1218 | element, set the parameter _cdata to 'true' 1219 | */ 1220 | TiXmlText (const char * initValue ) : TiXmlNode (TiXmlNode::TINYXML_TEXT) 1221 | { 1222 | SetValue( initValue ); 1223 | cdata = false; 1224 | } 1225 | virtual ~TiXmlText() {} 1226 | 1227 | #ifdef TIXML_USE_STL 1228 | /// Constructor. 1229 | TiXmlText( const std::string& initValue ) : TiXmlNode (TiXmlNode::TINYXML_TEXT) 1230 | { 1231 | SetValue( initValue ); 1232 | cdata = false; 1233 | } 1234 | #endif 1235 | 1236 | TiXmlText( const TiXmlText& copy ) : TiXmlNode( TiXmlNode::TINYXML_TEXT ) { copy.CopyTo( this ); } 1237 | TiXmlText& operator=( const TiXmlText& base ) { base.CopyTo( this ); return *this; } 1238 | 1239 | // Write this text object to a FILE stream. 1240 | virtual void Print( FILE* cfile, int depth ) const; 1241 | 1242 | /// Queries whether this represents text using a CDATA section. 1243 | bool CDATA() const { return cdata; } 1244 | /// Turns on or off a CDATA representation of text. 1245 | void SetCDATA( bool _cdata ) { cdata = _cdata; } 1246 | 1247 | virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); 1248 | 1249 | virtual const TiXmlText* ToText() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. 1250 | virtual TiXmlText* ToText() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. 1251 | 1252 | /** Walk the XML tree visiting this node and all of its children. 1253 | */ 1254 | virtual bool Accept( TiXmlVisitor* content ) const; 1255 | 1256 | protected : 1257 | /// [internal use] Creates a new Element and returns it. 1258 | virtual TiXmlNode* Clone() const; 1259 | void CopyTo( TiXmlText* target ) const; 1260 | 1261 | bool Blank() const; // returns true if all white space and new lines 1262 | // [internal use] 1263 | #ifdef TIXML_USE_STL 1264 | virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); 1265 | #endif 1266 | 1267 | private: 1268 | bool cdata; // true if this should be input and output as a CDATA style text element 1269 | }; 1270 | 1271 | 1272 | /** In correct XML the declaration is the first entry in the file. 1273 | @verbatim 1274 | 1275 | @endverbatim 1276 | 1277 | TinyXml will happily read or write files without a declaration, 1278 | however. There are 3 possible attributes to the declaration: 1279 | version, encoding, and standalone. 1280 | 1281 | Note: In this version of the code, the attributes are 1282 | handled as special cases, not generic attributes, simply 1283 | because there can only be at most 3 and they are always the same. 1284 | */ 1285 | class TiXmlDeclaration : public TiXmlNode 1286 | { 1287 | public: 1288 | /// Construct an empty declaration. 1289 | TiXmlDeclaration() : TiXmlNode( TiXmlNode::TINYXML_DECLARATION ) {} 1290 | 1291 | #ifdef TIXML_USE_STL 1292 | /// Constructor. 1293 | TiXmlDeclaration( const std::string& _version, 1294 | const std::string& _encoding, 1295 | const std::string& _standalone ); 1296 | #endif 1297 | 1298 | /// Construct. 1299 | TiXmlDeclaration( const char* _version, 1300 | const char* _encoding, 1301 | const char* _standalone ); 1302 | 1303 | TiXmlDeclaration( const TiXmlDeclaration& copy ); 1304 | TiXmlDeclaration& operator=( const TiXmlDeclaration& copy ); 1305 | 1306 | virtual ~TiXmlDeclaration() {} 1307 | 1308 | /// Version. Will return an empty string if none was found. 1309 | const char *Version() const { return version.c_str (); } 1310 | /// Encoding. Will return an empty string if none was found. 1311 | const char *Encoding() const { return encoding.c_str (); } 1312 | /// Is this a standalone document? 1313 | const char *Standalone() const { return standalone.c_str (); } 1314 | 1315 | /// Creates a copy of this Declaration and returns it. 1316 | virtual TiXmlNode* Clone() const; 1317 | // Print this declaration to a FILE stream. 1318 | virtual void Print( FILE* cfile, int depth, TIXML_STRING* str ) const; 1319 | virtual void Print( FILE* cfile, int depth ) const { 1320 | Print( cfile, depth, 0 ); 1321 | } 1322 | 1323 | virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); 1324 | 1325 | virtual const TiXmlDeclaration* ToDeclaration() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. 1326 | virtual TiXmlDeclaration* ToDeclaration() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. 1327 | 1328 | /** Walk the XML tree visiting this node and all of its children. 1329 | */ 1330 | virtual bool Accept( TiXmlVisitor* visitor ) const; 1331 | 1332 | protected: 1333 | void CopyTo( TiXmlDeclaration* target ) const; 1334 | // used to be public 1335 | #ifdef TIXML_USE_STL 1336 | virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); 1337 | #endif 1338 | 1339 | private: 1340 | 1341 | TIXML_STRING version; 1342 | TIXML_STRING encoding; 1343 | TIXML_STRING standalone; 1344 | }; 1345 | 1346 | 1347 | /** Any tag that tinyXml doesn't recognize is saved as an 1348 | unknown. It is a tag of text, but should not be modified. 1349 | It will be written back to the XML, unchanged, when the file 1350 | is saved. 1351 | 1352 | DTD tags get thrown into TiXmlUnknowns. 1353 | */ 1354 | class TiXmlUnknown : public TiXmlNode 1355 | { 1356 | public: 1357 | TiXmlUnknown() : TiXmlNode( TiXmlNode::TINYXML_UNKNOWN ) {} 1358 | virtual ~TiXmlUnknown() {} 1359 | 1360 | TiXmlUnknown( const TiXmlUnknown& copy ) : TiXmlNode( TiXmlNode::TINYXML_UNKNOWN ) { copy.CopyTo( this ); } 1361 | TiXmlUnknown& operator=( const TiXmlUnknown& copy ) { copy.CopyTo( this ); return *this; } 1362 | 1363 | /// Creates a copy of this Unknown and returns it. 1364 | virtual TiXmlNode* Clone() const; 1365 | // Print this Unknown to a FILE stream. 1366 | virtual void Print( FILE* cfile, int depth ) const; 1367 | 1368 | virtual const char* Parse( const char* p, TiXmlParsingData* data, TiXmlEncoding encoding ); 1369 | 1370 | virtual const TiXmlUnknown* ToUnknown() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. 1371 | virtual TiXmlUnknown* ToUnknown() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. 1372 | 1373 | /** Walk the XML tree visiting this node and all of its children. 1374 | */ 1375 | virtual bool Accept( TiXmlVisitor* content ) const; 1376 | 1377 | protected: 1378 | void CopyTo( TiXmlUnknown* target ) const; 1379 | 1380 | #ifdef TIXML_USE_STL 1381 | virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); 1382 | #endif 1383 | 1384 | private: 1385 | 1386 | }; 1387 | 1388 | 1389 | /** Always the top level node. A document binds together all the 1390 | XML pieces. It can be saved, loaded, and printed to the screen. 1391 | The 'value' of a document node is the xml file name. 1392 | */ 1393 | class TiXmlDocument : public TiXmlNode 1394 | { 1395 | public: 1396 | /// Create an empty document, that has no name. 1397 | TiXmlDocument(); 1398 | /// Create a document with a name. The name of the document is also the filename of the xml. 1399 | TiXmlDocument( const char * documentName ); 1400 | 1401 | #ifdef TIXML_USE_STL 1402 | /// Constructor. 1403 | TiXmlDocument( const std::string& documentName ); 1404 | #endif 1405 | 1406 | TiXmlDocument( const TiXmlDocument& copy ); 1407 | TiXmlDocument& operator=( const TiXmlDocument& copy ); 1408 | 1409 | virtual ~TiXmlDocument() {} 1410 | 1411 | /** Load a file using the current document value. 1412 | Returns true if successful. Will delete any existing 1413 | document data before loading. 1414 | */ 1415 | bool LoadFile( TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); 1416 | /// Save a file using the current document value. Returns true if successful. 1417 | bool SaveFile() const; 1418 | /// Load a file using the given filename. Returns true if successful. 1419 | bool LoadFile( const char * filename, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); 1420 | /// Save a file using the given filename. Returns true if successful. 1421 | bool SaveFile( const char * filename ) const; 1422 | /** Load a file using the given FILE*. Returns true if successful. Note that this method 1423 | doesn't stream - the entire object pointed at by the FILE* 1424 | will be interpreted as an XML file. TinyXML doesn't stream in XML from the current 1425 | file location. Streaming may be added in the future. 1426 | */ 1427 | bool LoadFile( FILE*, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); 1428 | /// Save a file using the given FILE*. Returns true if successful. 1429 | bool SaveFile( FILE* ) const; 1430 | 1431 | #ifdef TIXML_USE_STL 1432 | bool LoadFile( const std::string& filename, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ) ///< STL std::string version. 1433 | { 1434 | return LoadFile( filename.c_str(), encoding ); 1435 | } 1436 | bool SaveFile( const std::string& filename ) const ///< STL std::string version. 1437 | { 1438 | return SaveFile( filename.c_str() ); 1439 | } 1440 | #endif 1441 | 1442 | /** Parse the given null terminated block of xml data. Passing in an encoding to this 1443 | method (either TIXML_ENCODING_LEGACY or TIXML_ENCODING_UTF8 will force TinyXml 1444 | to use that encoding, regardless of what TinyXml might otherwise try to detect. 1445 | */ 1446 | virtual const char* Parse( const char* p, TiXmlParsingData* data = 0, TiXmlEncoding encoding = TIXML_DEFAULT_ENCODING ); 1447 | 1448 | /** Get the root element -- the only top level element -- of the document. 1449 | In well formed XML, there should only be one. TinyXml is tolerant of 1450 | multiple elements at the document level. 1451 | */ 1452 | const TiXmlElement* RootElement() const { return FirstChildElement(); } 1453 | TiXmlElement* RootElement() { return FirstChildElement(); } 1454 | 1455 | /** If an error occurs, Error will be set to true. Also, 1456 | - The ErrorId() will contain the integer identifier of the error (not generally useful) 1457 | - The ErrorDesc() method will return the name of the error. (very useful) 1458 | - The ErrorRow() and ErrorCol() will return the location of the error (if known) 1459 | */ 1460 | bool Error() const { return error; } 1461 | 1462 | /// Contains a textual (english) description of the error if one occurs. 1463 | const char * ErrorDesc() const { return errorDesc.c_str (); } 1464 | 1465 | /** Generally, you probably want the error string ( ErrorDesc() ). But if you 1466 | prefer the ErrorId, this function will fetch it. 1467 | */ 1468 | int ErrorId() const { return errorId; } 1469 | 1470 | /** Returns the location (if known) of the error. The first column is column 1, 1471 | and the first row is row 1. A value of 0 means the row and column wasn't applicable 1472 | (memory errors, for example, have no row/column) or the parser lost the error. (An 1473 | error in the error reporting, in that case.) 1474 | 1475 | @sa SetTabSize, Row, Column 1476 | */ 1477 | int ErrorRow() const { return errorLocation.row+1; } 1478 | int ErrorCol() const { return errorLocation.col+1; } ///< The column where the error occured. See ErrorRow() 1479 | 1480 | /** SetTabSize() allows the error reporting functions (ErrorRow() and ErrorCol()) 1481 | to report the correct values for row and column. It does not change the output 1482 | or input in any way. 1483 | 1484 | By calling this method, with a tab size 1485 | greater than 0, the row and column of each node and attribute is stored 1486 | when the file is loaded. Very useful for tracking the DOM back in to 1487 | the source file. 1488 | 1489 | The tab size is required for calculating the location of nodes. If not 1490 | set, the default of 4 is used. The tabsize is set per document. Setting 1491 | the tabsize to 0 disables row/column tracking. 1492 | 1493 | Note that row and column tracking is not supported when using operator>>. 1494 | 1495 | The tab size needs to be enabled before the parse or load. Correct usage: 1496 | @verbatim 1497 | TiXmlDocument doc; 1498 | doc.SetTabSize( 8 ); 1499 | doc.Load( "myfile.xml" ); 1500 | @endverbatim 1501 | 1502 | @sa Row, Column 1503 | */ 1504 | void SetTabSize( int _tabsize ) { tabsize = _tabsize; } 1505 | 1506 | int TabSize() const { return tabsize; } 1507 | 1508 | /** If you have handled the error, it can be reset with this call. The error 1509 | state is automatically cleared if you Parse a new XML block. 1510 | */ 1511 | void ClearError() { error = false; 1512 | errorId = 0; 1513 | errorDesc = ""; 1514 | errorLocation.row = errorLocation.col = 0; 1515 | //errorLocation.last = 0; 1516 | } 1517 | 1518 | /** Write the document to standard out using formatted printing ("pretty print"). */ 1519 | void Print() const { Print( stdout, 0 ); } 1520 | 1521 | /* Write the document to a string using formatted printing ("pretty print"). This 1522 | will allocate a character array (new char[]) and return it as a pointer. The 1523 | calling code pust call delete[] on the return char* to avoid a memory leak. 1524 | */ 1525 | //char* PrintToMemory() const; 1526 | 1527 | /// Print this Document to a FILE stream. 1528 | virtual void Print( FILE* cfile, int depth = 0 ) const; 1529 | // [internal use] 1530 | void SetError( int err, const char* errorLocation, TiXmlParsingData* prevData, TiXmlEncoding encoding ); 1531 | 1532 | virtual const TiXmlDocument* ToDocument() const { return this; } ///< Cast to a more defined type. Will return null not of the requested type. 1533 | virtual TiXmlDocument* ToDocument() { return this; } ///< Cast to a more defined type. Will return null not of the requested type. 1534 | 1535 | /** Walk the XML tree visiting this node and all of its children. 1536 | */ 1537 | virtual bool Accept( TiXmlVisitor* content ) const; 1538 | 1539 | protected : 1540 | // [internal use] 1541 | virtual TiXmlNode* Clone() const; 1542 | #ifdef TIXML_USE_STL 1543 | virtual void StreamIn( std::istream * in, TIXML_STRING * tag ); 1544 | #endif 1545 | 1546 | private: 1547 | void CopyTo( TiXmlDocument* target ) const; 1548 | 1549 | bool error; 1550 | int errorId; 1551 | TIXML_STRING errorDesc; 1552 | int tabsize; 1553 | TiXmlCursor errorLocation; 1554 | bool useMicrosoftBOM; // the UTF-8 BOM were found when read. Note this, and try to write. 1555 | }; 1556 | 1557 | 1558 | /** 1559 | A TiXmlHandle is a class that wraps a node pointer with null checks; this is 1560 | an incredibly useful thing. Note that TiXmlHandle is not part of the TinyXml 1561 | DOM structure. It is a separate utility class. 1562 | 1563 | Take an example: 1564 | @verbatim 1565 | 1566 | 1567 | 1568 | 1569 | 1570 | 1571 | @endverbatim 1572 | 1573 | Assuming you want the value of "attributeB" in the 2nd "Child" element, it's very 1574 | easy to write a *lot* of code that looks like: 1575 | 1576 | @verbatim 1577 | TiXmlElement* root = document.FirstChildElement( "Document" ); 1578 | if ( root ) 1579 | { 1580 | TiXmlElement* element = root->FirstChildElement( "Element" ); 1581 | if ( element ) 1582 | { 1583 | TiXmlElement* child = element->FirstChildElement( "Child" ); 1584 | if ( child ) 1585 | { 1586 | TiXmlElement* child2 = child->NextSiblingElement( "Child" ); 1587 | if ( child2 ) 1588 | { 1589 | // Finally do something useful. 1590 | @endverbatim 1591 | 1592 | And that doesn't even cover "else" cases. TiXmlHandle addresses the verbosity 1593 | of such code. A TiXmlHandle checks for null pointers so it is perfectly safe 1594 | and correct to use: 1595 | 1596 | @verbatim 1597 | TiXmlHandle docHandle( &document ); 1598 | TiXmlElement* child2 = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).Child( "Child", 1 ).ToElement(); 1599 | if ( child2 ) 1600 | { 1601 | // do something useful 1602 | @endverbatim 1603 | 1604 | Which is MUCH more concise and useful. 1605 | 1606 | It is also safe to copy handles - internally they are nothing more than node pointers. 1607 | @verbatim 1608 | TiXmlHandle handleCopy = handle; 1609 | @endverbatim 1610 | 1611 | What they should not be used for is iteration: 1612 | 1613 | @verbatim 1614 | int i=0; 1615 | while ( true ) 1616 | { 1617 | TiXmlElement* child = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).Child( "Child", i ).ToElement(); 1618 | if ( !child ) 1619 | break; 1620 | // do something 1621 | ++i; 1622 | } 1623 | @endverbatim 1624 | 1625 | It seems reasonable, but it is in fact two embedded while loops. The Child method is 1626 | a linear walk to find the element, so this code would iterate much more than it needs 1627 | to. Instead, prefer: 1628 | 1629 | @verbatim 1630 | TiXmlElement* child = docHandle.FirstChild( "Document" ).FirstChild( "Element" ).FirstChild( "Child" ).ToElement(); 1631 | 1632 | for( child; child; child=child->NextSiblingElement() ) 1633 | { 1634 | // do something 1635 | } 1636 | @endverbatim 1637 | */ 1638 | class TiXmlHandle 1639 | { 1640 | public: 1641 | /// Create a handle from any node (at any depth of the tree.) This can be a null pointer. 1642 | TiXmlHandle( TiXmlNode* _node ) { this->node = _node; } 1643 | /// Copy constructor 1644 | TiXmlHandle( const TiXmlHandle& ref ) { this->node = ref.node; } 1645 | TiXmlHandle operator=( const TiXmlHandle& ref ) { if ( &ref != this ) this->node = ref.node; return *this; } 1646 | 1647 | /// Return a handle to the first child node. 1648 | TiXmlHandle FirstChild() const; 1649 | /// Return a handle to the first child node with the given name. 1650 | TiXmlHandle FirstChild( const char * value ) const; 1651 | /// Return a handle to the first child element. 1652 | TiXmlHandle FirstChildElement() const; 1653 | /// Return a handle to the first child element with the given name. 1654 | TiXmlHandle FirstChildElement( const char * value ) const; 1655 | 1656 | /** Return a handle to the "index" child with the given name. 1657 | The first child is 0, the second 1, etc. 1658 | */ 1659 | TiXmlHandle Child( const char* value, int index ) const; 1660 | /** Return a handle to the "index" child. 1661 | The first child is 0, the second 1, etc. 1662 | */ 1663 | TiXmlHandle Child( int index ) const; 1664 | /** Return a handle to the "index" child element with the given name. 1665 | The first child element is 0, the second 1, etc. Note that only TiXmlElements 1666 | are indexed: other types are not counted. 1667 | */ 1668 | TiXmlHandle ChildElement( const char* value, int index ) const; 1669 | /** Return a handle to the "index" child element. 1670 | The first child element is 0, the second 1, etc. Note that only TiXmlElements 1671 | are indexed: other types are not counted. 1672 | */ 1673 | TiXmlHandle ChildElement( int index ) const; 1674 | 1675 | #ifdef TIXML_USE_STL 1676 | TiXmlHandle FirstChild( const std::string& _value ) const { return FirstChild( _value.c_str() ); } 1677 | TiXmlHandle FirstChildElement( const std::string& _value ) const { return FirstChildElement( _value.c_str() ); } 1678 | 1679 | TiXmlHandle Child( const std::string& _value, int index ) const { return Child( _value.c_str(), index ); } 1680 | TiXmlHandle ChildElement( const std::string& _value, int index ) const { return ChildElement( _value.c_str(), index ); } 1681 | #endif 1682 | 1683 | /** Return the handle as a TiXmlNode. This may return null. 1684 | */ 1685 | TiXmlNode* ToNode() const { return node; } 1686 | /** Return the handle as a TiXmlElement. This may return null. 1687 | */ 1688 | TiXmlElement* ToElement() const { return ( ( node && node->ToElement() ) ? node->ToElement() : 0 ); } 1689 | /** Return the handle as a TiXmlText. This may return null. 1690 | */ 1691 | TiXmlText* ToText() const { return ( ( node && node->ToText() ) ? node->ToText() : 0 ); } 1692 | /** Return the handle as a TiXmlUnknown. This may return null. 1693 | */ 1694 | TiXmlUnknown* ToUnknown() const { return ( ( node && node->ToUnknown() ) ? node->ToUnknown() : 0 ); } 1695 | 1696 | /** @deprecated use ToNode. 1697 | Return the handle as a TiXmlNode. This may return null. 1698 | */ 1699 | TiXmlNode* Node() const { return ToNode(); } 1700 | /** @deprecated use ToElement. 1701 | Return the handle as a TiXmlElement. This may return null. 1702 | */ 1703 | TiXmlElement* Element() const { return ToElement(); } 1704 | /** @deprecated use ToText() 1705 | Return the handle as a TiXmlText. This may return null. 1706 | */ 1707 | TiXmlText* Text() const { return ToText(); } 1708 | /** @deprecated use ToUnknown() 1709 | Return the handle as a TiXmlUnknown. This may return null. 1710 | */ 1711 | TiXmlUnknown* Unknown() const { return ToUnknown(); } 1712 | 1713 | private: 1714 | TiXmlNode* node; 1715 | }; 1716 | 1717 | 1718 | /** Print to memory functionality. The TiXmlPrinter is useful when you need to: 1719 | 1720 | -# Print to memory (especially in non-STL mode) 1721 | -# Control formatting (line endings, etc.) 1722 | 1723 | When constructed, the TiXmlPrinter is in its default "pretty printing" mode. 1724 | Before calling Accept() you can call methods to control the printing 1725 | of the XML document. After TiXmlNode::Accept() is called, the printed document can 1726 | be accessed via the CStr(), Str(), and Size() methods. 1727 | 1728 | TiXmlPrinter uses the Visitor API. 1729 | @verbatim 1730 | TiXmlPrinter printer; 1731 | printer.SetIndent( "\t" ); 1732 | 1733 | doc.Accept( &printer ); 1734 | fprintf( stdout, "%s", printer.CStr() ); 1735 | @endverbatim 1736 | */ 1737 | class TiXmlPrinter : public TiXmlVisitor 1738 | { 1739 | public: 1740 | TiXmlPrinter() : depth( 0 ), simpleTextPrint( false ), 1741 | buffer(), indent( " " ), lineBreak( "\n" ) {} 1742 | 1743 | virtual bool VisitEnter( const TiXmlDocument& doc ); 1744 | virtual bool VisitExit( const TiXmlDocument& doc ); 1745 | 1746 | virtual bool VisitEnter( const TiXmlElement& element, const TiXmlAttribute* firstAttribute ); 1747 | virtual bool VisitExit( const TiXmlElement& element ); 1748 | 1749 | virtual bool Visit( const TiXmlDeclaration& declaration ); 1750 | virtual bool Visit( const TiXmlText& text ); 1751 | virtual bool Visit( const TiXmlComment& comment ); 1752 | virtual bool Visit( const TiXmlUnknown& unknown ); 1753 | 1754 | /** Set the indent characters for printing. By default 4 spaces 1755 | but tab (\t) is also useful, or null/empty string for no indentation. 1756 | */ 1757 | void SetIndent( const char* _indent ) { indent = _indent ? _indent : "" ; } 1758 | /// Query the indention string. 1759 | const char* Indent() { return indent.c_str(); } 1760 | /** Set the line breaking string. By default set to newline (\n). 1761 | Some operating systems prefer other characters, or can be 1762 | set to the null/empty string for no indenation. 1763 | */ 1764 | void SetLineBreak( const char* _lineBreak ) { lineBreak = _lineBreak ? _lineBreak : ""; } 1765 | /// Query the current line breaking string. 1766 | const char* LineBreak() { return lineBreak.c_str(); } 1767 | 1768 | /** Switch over to "stream printing" which is the most dense formatting without 1769 | linebreaks. Common when the XML is needed for network transmission. 1770 | */ 1771 | void SetStreamPrinting() { indent = ""; 1772 | lineBreak = ""; 1773 | } 1774 | /// Return the result. 1775 | const char* CStr() { return buffer.c_str(); } 1776 | /// Return the length of the result string. 1777 | size_t Size() { return buffer.size(); } 1778 | 1779 | #ifdef TIXML_USE_STL 1780 | /// Return the result. 1781 | const std::string& Str() { return buffer; } 1782 | #endif 1783 | 1784 | private: 1785 | void DoIndent() { 1786 | for( int i=0; i 5 | #include 6 | #include 7 | #include "control/CtrlComponents.h" 8 | #include "trajectory/SCurve.h" 9 | 10 | 11 | class JointSpaceTraj : public Trajectory{ 12 | public: 13 | JointSpaceTraj(CtrlComponents *ctrlComp); 14 | ~JointSpaceTraj(){} 15 | 16 | bool getJointCmd(Vec6 &q, Vec6 &qd); 17 | bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd); 18 | 19 | void setGripper(double startQ, double endQ, double speed = M_PI); 20 | void setJointTraj(Vec6 startQ, Vec6 endQ, double speed); 21 | bool setJointTraj(Vec6 startQ, std::string endName, double speed); 22 | bool setJointTraj(std::string startName, std::string endName, double speed); 23 | private: 24 | void _generateA345(double pathTime); 25 | 26 | SCurve _jointCurve; 27 | const double ddQMax = 15.0; 28 | const double dddQMax = 30.0; 29 | 30 | double _a3, _a4, _a5, _s, _sDot; 31 | }; 32 | 33 | #endif // JOINTSPACETRAJ_H -------------------------------------------------------------------------------- /include/trajectory/SCurve.h: -------------------------------------------------------------------------------- 1 | #ifndef SCURVE_H 2 | #define SCURVE_H 3 | 4 | /* 归一化后的s曲线 */ 5 | 6 | #include 7 | 8 | class SCurve{ 9 | public: 10 | SCurve(){} 11 | ~SCurve(){} 12 | void setSCurve(double deltaQ, double dQMax, double ddQMax, double dddQMax); 13 | void restart(); 14 | /* 15 | 考虑到归一化，在实际使用时，物理量为： 16 | 加速度 = deltaQ * getDDs(); 17 | */ 18 | double getDDs(); 19 | double getDDs(double t); 20 | /* 21 | 考虑到归一化，在实际使用时，物理量为： 22 | 速度 = deltaQ * getDs(); 23 | */ 24 | double getDs(); 25 | double getDs(double t); 26 | /* 27 | 考虑到归一化，在实际使用时，物理量为： 28 | 位置 = deltaQ * gets(); 29 | */ 30 | double gets(); 31 | double gets(double t); 32 | 33 | double getT(); 34 | private: 35 | int _getSegment(double t); 36 | void _setFunc(); 37 | double _runTime(); 38 | 39 | bool _started = false; 40 | double _startTime; 41 | 42 | double _J, _aMax, _vMax; 43 | double _T[7]; // period 44 | double _t[7]; // moment 45 | double _v0, _v1; // ds at _t[0], _t[1] 46 | double _s0, _s1, _s2, _s3, _s4, _s5, _s6; // s at _t[0], _t[1] 47 | }; 48 | 49 | #endif // SCURVE_H -------------------------------------------------------------------------------- /include/trajectory/StopForTime.h: -------------------------------------------------------------------------------- 1 | #ifndef STOPFORTIME_H 2 | #define STOPFORTIME_H 3 | 4 | #include "trajectory/Trajectory.h" 5 | 6 | class StopForTime : public Trajectory{ 7 | public: 8 | StopForTime(CtrlComponents *ctrlComp); 9 | ~StopForTime(){} 10 | 11 | bool getJointCmd(Vec6 &q, Vec6 &qd); 12 | bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd); 13 | void setStop(Vec6 stopQ, double stopTime); 14 | private: 15 | }; 16 | 17 | #endif // STOPFORTIME_H -------------------------------------------------------------------------------- /include/trajectory/Trajectory.h: -------------------------------------------------------------------------------- 1 | #ifndef TRAJECTORY_H 2 | #define TRAJECTORY_H 3 | 4 | #include "common/math/mathTools.h" 5 | #include "control/CtrlComponents.h" 6 | #include "common/utilities/timer.h" 7 | 8 | class Trajectory{ 9 | public: 10 | Trajectory(CtrlComponents *ctrlComp); 11 | virtual ~Trajectory(){} 12 | virtual bool getJointCmd(Vec6 &q, Vec6 &qd){return false;}; 13 | virtual bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd){return false;}; 14 | virtual bool getCartesionCmd(Vec6 pastPosture, Vec6 &endPosture, Vec6 &endTwist){return false;}; 15 | 16 | void restart(); 17 | virtual void setGripper(double startQ, double endQ, double speed = M_PI); 18 | 19 | bool correctYN(){return _settingCorrect;} 20 | Vec6 getStartQ(){return _startQ;} 21 | Vec6 getEndQ(){return _endQ;} 22 | double getEndGripperQ(){return _endGripperQ;}; 23 | double getStartGripperQ(){return _startGripperQ;}; 24 | HomoMat getStartHomo(){return _startHomo;}; 25 | HomoMat getEndHomo(){return _endHomo;}; 26 | Vec6 getEndPosture(){return _endPosture;}; 27 | double getPathTime(){return _pathTime;} 28 | protected: 29 | void _runTime(); 30 | 31 | CtrlComponents *_ctrlComp; 32 | ArmModel *_armModel; 33 | CSVTool *_csvState; 34 | bool _pathStarted = false; 35 | bool _reached = false; 36 | bool _settingCorrect = true; 37 | double _startTime, _currentTime, _pathTime, _tCost; 38 | 39 | Vec6 _qPast; 40 | Vec6 _startQ, _endQ, _deltaQ; 41 | HomoMat _startHomo, _endHomo; 42 | Vec6 _startPosture, _endPosture, _deltaPosture; 43 | double _startGripperQ, _endGripperQ; 44 | }; 45 | 46 | #endif // TRAJECTORY_H -------------------------------------------------------------------------------- /include/trajectory/TrajectoryManager.h: -------------------------------------------------------------------------------- 1 | #ifndef TRAJECTORY_MANAGER_H 2 | #define TRAJECTORY_MANAGER_H 3 | 4 | #include 5 | #include "control/CtrlComponents.h" 6 | #include "trajectory/JointSpaceTraj.h" 7 | #include "trajectory/EndLineTraj.h" 8 | #include "trajectory/EndCircleTraj.h" 9 | #include "trajectory/StopForTime.h" 10 | 11 | class TrajectoryManager{ 12 | public: 13 | TrajectoryManager(CtrlComponents *ctrlComp); 14 | ~TrajectoryManager(){} 15 | bool getJointCmd(Vec6 &q, Vec6 &qd); 16 | bool getJointCmd(Vec6 &q, Vec6 &qd, double &gripperQ, double &gripperQd); 17 | void addTrajectory(Trajectory* traj); 18 | void setLoop(double backSpeed = 0.7); 19 | void restartTraj(); 20 | void emptyTraj(); 21 | Vec6 getStartQ(); 22 | Vec6 getEndQ(); 23 | Vec6 getEndPosture(); 24 | double getStartGripperQ(); 25 | double getEndGripperQ(); 26 | HomoMat getEndHomo(); 27 | size_t size() {return _trajVec.size();} ; 28 | private: 29 | CtrlComponents *_ctrlComp; 30 | JointSpaceTraj *_trajBack; 31 | std::vector _trajVec; 32 | int _trajID = 0; 33 | double _jointErr = 0.05; 34 | bool _trajCorrect = true; 35 | bool _loop = false; 36 | }; 37 | 38 | #endif // TRAJECTORY_MANAGER_H -------------------------------------------------------------------------------- /lib/libZ1_aarch64.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/z1_controller/b6fd77090b3ae9d4d6b7c7ad2a93373a62d70a14/lib/libZ1_aarch64.so -------------------------------------------------------------------------------- /lib/libZ1_x86_64.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/z1_controller/b6fd77090b3ae9d4d6b7c7ad2a93373a62d70a14/lib/libZ1_x86_64.so -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "FSM/FiniteStateMachine.h" 4 | #include "FSM/State_Passive.h" 5 | #include "FSM/State_BackToStart.h" 6 | #include "FSM/State_Calibration.h" 7 | #include "FSM/State_Cartesian.h" 8 | #include "FSM/State_JointSpace.h" 9 | #include "FSM/State_MoveJ.h" 10 | #include "FSM/State_MoveL.h" 11 | #include "FSM/State_MoveC.h" 12 | #include "FSM/State_ToState.h" 13 | #include "FSM/State_SaveState.h" 14 | #include "FSM/State_Teach.h" 15 | #include "FSM/State_TeachRepeat.h" 16 | #include "FSM/State_Trajectory.h" 17 | #include "FSM/State_LowCmd.h" 18 | 19 | bool running = true; 20 | 21 | //set real-time program 22 | void setProcessScheduler(){ 23 | pid_t pid = getpid(); 24 | sched_param param; 25 | param.sched_priority = sched_get_priority_max(SCHED_FIFO); 26 | if (sched_setscheduler(pid, SCHED_FIFO, ¶m) == -1) { 27 | // std::cout << "[ERROR] Function setProcessScheduler failed." << std::endl; 28 | } 29 | } 30 | 31 | int main(int argc, char **argv){ 32 | /* set real-time process */ 33 | setProcessScheduler(); 34 | /* set the print format */ 35 | std::cout << std::fixed << std::setprecision(5); 36 | 37 | 38 | EmptyAction emptyAction((int)ArmFSMStateName::INVALID); 39 | std::vector events; 40 | CtrlComponents *ctrlComp = new CtrlComponents(argc, argv); 41 | 42 | ctrlComp->dt = 1.0/250.; 43 | ctrlComp->armConfigPath = "../config/"; 44 | ctrlComp->stateCSV = new CSVTool("../config/savedArmStates.csv"); 45 | ctrlComp->ioInter = new IOUDP(ctrlComp->ctrl_IP.c_str(), ctrlComp->ctrl_port); 46 | ctrlComp->geneObj(); 47 | if(ctrlComp->ctrl == Control::SDK){ 48 | ctrlComp->cmdPanel = new ARMSDK(events, emptyAction, "127.0.0.1", 8072, 8071, 0.002); 49 | }else if(ctrlComp->ctrl == Control::KEYBOARD){ 50 | events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART)); 51 | events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE)); 52 | events.push_back(new StateAction("2", (int)ArmFSMStateName::JOINTCTRL)); 53 | events.push_back(new StateAction("3", (int)ArmFSMStateName::CARTESIAN)); 54 | events.push_back(new StateAction("4", (int)ArmFSMStateName::MOVEJ)); 55 | events.push_back(new StateAction("5", (int)ArmFSMStateName::MOVEL)); 56 | events.push_back(new StateAction("6", (int)ArmFSMStateName::MOVEC)); 57 | events.push_back(new StateAction("7", (int)ArmFSMStateName::TEACH)); 58 | events.push_back(new StateAction("8", (int)ArmFSMStateName::TEACHREPEAT)); 59 | events.push_back(new StateAction("9", (int)ArmFSMStateName::SAVESTATE)); 60 | events.push_back(new StateAction("0", (int)ArmFSMStateName::TOSTATE)); 61 | events.push_back(new StateAction("-", (int)ArmFSMStateName::TRAJECTORY)); 62 | events.push_back(new StateAction("=", (int)ArmFSMStateName::CALIBRATION)); 63 | 64 | events.push_back(new ValueAction("q", "a", 0.5)); 65 | events.push_back(new ValueAction("w", "s", 0.5)); 66 | events.push_back(new ValueAction("e", "d", 0.5)); 67 | events.push_back(new ValueAction("r", "f", 0.5)); 68 | events.push_back(new ValueAction("t", "g", 0.5)); 69 | events.push_back(new ValueAction("y", "h", 0.5)); 70 | events.push_back(new ValueAction("down", "up", 1.)); 71 | 72 | ctrlComp->cmdPanel = new Keyboard(events, emptyAction); 73 | } 74 | std::vector states; 75 | states.push_back(new State_Passive(ctrlComp)); 76 | states.push_back(new State_BackToStart(ctrlComp)); 77 | states.push_back(new State_JointSpace(ctrlComp)); 78 | states.push_back(new State_Cartesian(ctrlComp)); 79 | states.push_back(new State_MoveJ(ctrlComp)); 80 | states.push_back(new State_MoveL(ctrlComp)); 81 | states.push_back(new State_MoveC(ctrlComp)); 82 | states.push_back(new State_LowCmd(ctrlComp)); 83 | states.push_back(new State_SaveState(ctrlComp)); 84 | states.push_back(new State_Teach(ctrlComp)); 85 | states.push_back(new State_TeachRepeat(ctrlComp)); 86 | states.push_back(new State_ToState(ctrlComp)); 87 | states.push_back(new State_Trajectory(ctrlComp)); 88 | states.push_back(new State_Calibration(ctrlComp)); 89 | 90 | FiniteStateMachine *fsm; 91 | fsm = new FiniteStateMachine(states, ctrlComp); 92 | 93 | ctrlComp->running = &running; 94 | signal(SIGINT, [](int signum){running = false;}); 95 | std::this_thread::sleep_for(std::chrono::seconds(1)); 96 | ctrlComp->cmdPanel->start(); 97 | while(running){ 98 | std::this_thread::sleep_for(std::chrono::seconds(1)); 99 | } 100 | 101 | delete fsm; 102 | delete ctrlComp; 103 | return 0; 104 | } -------------------------------------------------------------------------------- /sim/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(catkin REQUIRED COMPONENTS 2 | controller_manager 3 | genmsg 4 | joint_state_controller 5 | robot_state_publisher 6 | roscpp 7 | gazebo_ros 8 | std_msgs 9 | tf 10 | geometry_msgs 11 | ) 12 | 13 | catkin_package( 14 | CATKIN_DEPENDS 15 | ) 16 | 17 | include_directories( 18 | ${catkin_INCLUDE_DIRS} 19 | ${GAZEBO_INCLUDE_DIRS} 20 | sim 21 | ) 22 | 23 | link_directories( 24 | ${GAZEBO_LIBRARY_DIRS} 25 | ) 26 | 27 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") 28 | 29 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}) 30 | add_executable(sim_ctrl sim_ctrl.cpp IOROS.cpp) 31 | target_link_libraries(sim_ctrl 32 | libZ1_${CMAKE_HOST_SYSTEM_PROCESSOR}.so 33 | ${catkin_LIBRARIES} 34 | ) -------------------------------------------------------------------------------- /sim/IOROS.cpp: -------------------------------------------------------------------------------- 1 | #include "IOROS.h" 2 | #include 3 | #include 4 | #include 5 | 6 | void RosShutDown(int sig){ 7 | ROS_INFO("ROS interface shutting down!"); 8 | ros::shutdown(); 9 | } 10 | 11 | IOROS::IOROS(){ 12 | std::cout << "The control interface for ROS Gazebo simulation" << std::endl; 13 | hasGripper = false; 14 | /* start subscriber */ 15 | _initRecv(); 16 | ros::AsyncSpinner subSpinner(1); // one threads 17 | subSpinner.start(); 18 | usleep(300000); //wait for subscribers start 19 | /* initialize publisher */ 20 | _initSend(); 21 | 22 | signal(SIGINT, RosShutDown); 23 | 24 | //check if there is UnitreeGripper 25 | unitree_legged_msgs::MotorCmd grippercmd; 26 | grippercmd.mode = 10; 27 | grippercmd.q = 0; 28 | grippercmd.dq = 0; 29 | grippercmd.tau = 0; 30 | grippercmd.Kp = 0; 31 | grippercmd.Kd = 0; 32 | _servo_pub[6].publish(grippercmd); 33 | ros::spinOnce(); 34 | } 35 | 36 | IOROS::~IOROS(){ 37 | } 38 | 39 | bool IOROS::sendRecv(const LowlevelCmd *cmd, LowlevelState *state){ 40 | _sendCmd(cmd); 41 | _recvState(state); 42 | return true; 43 | } 44 | 45 | void IOROS::_sendCmd(const LowlevelCmd *lowCmd){ 46 | for(int i(0); i < lowCmd->q.size(); ++i){ 47 | _joint_cmd[i].mode = 10; 48 | _joint_cmd[i].q = lowCmd->q[i]; 49 | _joint_cmd[i].dq = lowCmd->dq[i]; 50 | _joint_cmd[i].tau = lowCmd->tau[i]; 51 | _joint_cmd[i].Kd = lowCmd->kd[i]*0.0128; 52 | _joint_cmd[i].Kp = lowCmd->kp[i]*25.6; 53 | _servo_pub[i].publish(_joint_cmd[i]); 54 | } 55 | ros::spinOnce(); 56 | } 57 | 58 | void IOROS::_recvState(LowlevelState *state){ 59 | for(int i(0); i < state->q.size(); ++i){ 60 | state->q[i] = _joint_state[i].q; 61 | state->dq[i] = _joint_state[i].dq; 62 | state->ddq[i] = _joint_state[i].ddq; 63 | state->tau[i] = _joint_state[i].tauEst; 64 | } 65 | } 66 | 67 | void IOROS::_initSend(){ 68 | _servo_pub[0] = _nm.advertise("/z1_gazebo/Joint01_controller/command", 1); 69 | _servo_pub[1] = _nm.advertise("/z1_gazebo/Joint02_controller/command", 1); 70 | _servo_pub[2] = _nm.advertise("/z1_gazebo/Joint03_controller/command", 1); 71 | _servo_pub[3] = _nm.advertise("/z1_gazebo/Joint04_controller/command", 1); 72 | _servo_pub[4] = _nm.advertise("/z1_gazebo/Joint05_controller/command", 1); 73 | _servo_pub[5] = _nm.advertise("/z1_gazebo/Joint06_controller/command", 1); 74 | _servo_pub[6] = _nm.advertise("/z1_gazebo/gripper_controller/command", 1); 75 | } 76 | 77 | void IOROS::_initRecv(){ 78 | _servo_sub[0] = _nm.subscribe("/z1_gazebo/Joint01_controller/state", 1, &IOROS::_joint00Callback, this); 79 | _servo_sub[1] = _nm.subscribe("/z1_gazebo/Joint02_controller/state", 1, &IOROS::_joint01Callback, this); 80 | _servo_sub[2] = _nm.subscribe("/z1_gazebo/Joint03_controller/state", 1, &IOROS::_joint02Callback, this); 81 | _servo_sub[3] = _nm.subscribe("/z1_gazebo/Joint04_controller/state", 1, &IOROS::_joint03Callback, this); 82 | _servo_sub[4] = _nm.subscribe("/z1_gazebo/Joint05_controller/state", 1, &IOROS::_joint04Callback, this); 83 | _servo_sub[5] = _nm.subscribe("/z1_gazebo/Joint06_controller/state", 1, &IOROS::_joint05Callback, this); 84 | _servo_sub[6] = _nm.subscribe("/z1_gazebo/gripper_controller/state", 1, &IOROS::_gripperCallback, this); 85 | } 86 | 87 | void IOROS::_joint00Callback(const unitree_legged_msgs::MotorState& msg){ 88 | _joint_state[0].mode = msg.mode; 89 | _joint_state[0].q = msg.q; 90 | _joint_state[0].dq = msg.dq; 91 | _joint_state[0].ddq = msg.ddq; 92 | _joint_state[0].tauEst = msg.tauEst; 93 | } 94 | 95 | void IOROS::_joint01Callback(const unitree_legged_msgs::MotorState& msg){ 96 | _joint_state[1].mode = msg.mode; 97 | _joint_state[1].q = msg.q; 98 | _joint_state[1].dq = msg.dq; 99 | _joint_state[1].ddq = msg.ddq; 100 | _joint_state[1].tauEst = msg.tauEst; 101 | } 102 | 103 | void IOROS::_joint02Callback(const unitree_legged_msgs::MotorState& msg){ 104 | _joint_state[2].mode = msg.mode; 105 | _joint_state[2].q = msg.q; 106 | _joint_state[2].dq = msg.dq; 107 | _joint_state[2].ddq = msg.ddq; 108 | _joint_state[2].tauEst = msg.tauEst; 109 | } 110 | 111 | void IOROS::_joint03Callback(const unitree_legged_msgs::MotorState& msg){ 112 | _joint_state[3].mode = msg.mode; 113 | _joint_state[3].q = msg.q; 114 | _joint_state[3].dq = msg.dq; 115 | _joint_state[3].ddq = msg.ddq; 116 | _joint_state[3].tauEst = msg.tauEst; 117 | } 118 | 119 | void IOROS::_joint04Callback(const unitree_legged_msgs::MotorState& msg){ 120 | _joint_state[4].mode = msg.mode; 121 | _joint_state[4].q = msg.q; 122 | _joint_state[4].dq = msg.dq; 123 | _joint_state[4].ddq = msg.ddq; 124 | _joint_state[4].tauEst = msg.tauEst; 125 | } 126 | 127 | void IOROS::_joint05Callback(const unitree_legged_msgs::MotorState& msg){ 128 | _joint_state[5].mode = msg.mode; 129 | _joint_state[5].q = msg.q; 130 | _joint_state[5].dq = msg.dq; 131 | _joint_state[5].ddq = msg.ddq; 132 | _joint_state[5].tauEst = msg.tauEst; 133 | } 134 | 135 | void IOROS::_gripperCallback(const unitree_legged_msgs::MotorState& msg){ 136 | hasGripper = true; 137 | _joint_state[6].mode = msg.mode; 138 | _joint_state[6].q = msg.q; 139 | _joint_state[6].dq = msg.dq; 140 | _joint_state[6].ddq = msg.ddq; 141 | _joint_state[6].tauEst = msg.tauEst; 142 | } -------------------------------------------------------------------------------- /sim/IOROS.h: -------------------------------------------------------------------------------- 1 | #ifndef IOROS_H 2 | #define IOROS_H 3 | 4 | #include 5 | #include "interface/IOInterface.h" 6 | #include "message/MotorCmd.h" 7 | #include "message/MotorState.h" 8 | 9 | class IOROS : public IOInterface{ 10 | public: 11 | IOROS(); 12 | ~IOROS(); 13 | bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state); 14 | private: 15 | ros::NodeHandle _nm; 16 | ros::Subscriber _servo_sub[7]; 17 | ros::Publisher _servo_pub[7]; 18 | unitree_legged_msgs::MotorState _joint_state[7]; 19 | unitree_legged_msgs::MotorCmd _joint_cmd[7]; 20 | void _sendCmd(const LowlevelCmd *cmd); 21 | void _recvState(LowlevelState *state); 22 | void _initRecv(); 23 | void _initSend(); 24 | 25 | void _joint00Callback(const unitree_legged_msgs::MotorState& msg); 26 | void _joint01Callback(const unitree_legged_msgs::MotorState& msg); 27 | void _joint02Callback(const unitree_legged_msgs::MotorState& msg); 28 | void _joint03Callback(const unitree_legged_msgs::MotorState& msg); 29 | void _joint04Callback(const unitree_legged_msgs::MotorState& msg); 30 | void _joint05Callback(const unitree_legged_msgs::MotorState& msg); 31 | void _gripperCallback(const unitree_legged_msgs::MotorState& msg); 32 | }; 33 | 34 | #endif // IOROS_H -------------------------------------------------------------------------------- /sim/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | z1_controller 4 | 0.0.0 5 | The z1_controller package 6 | 7 | unitree 8 | 9 | TODO 10 | 11 | catkin 12 | roscp 13 | urdf 14 | xacro 15 | roscp 16 | urdf 17 | xacro 18 | roscp 19 | urdf 20 | xacro 21 | 22 | 23 | -------------------------------------------------------------------------------- /sim/sim_ctrl.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "FSM/FiniteStateMachine.h" 4 | #include "FSM/State_Passive.h" 5 | #include "FSM/State_BackToStart.h" 6 | #include "FSM/State_Calibration.h" 7 | #include "FSM/State_Cartesian.h" 8 | #include "FSM/State_JointSpace.h" 9 | #include "FSM/State_MoveJ.h" 10 | #include "FSM/State_MoveL.h" 11 | #include "FSM/State_MoveC.h" 12 | #include "FSM/State_ToState.h" 13 | #include "FSM/State_SaveState.h" 14 | #include "FSM/State_Teach.h" 15 | #include "FSM/State_TeachRepeat.h" 16 | #include "FSM/State_Trajectory.h" 17 | #include "FSM/State_LowCmd.h" 18 | #include "IOROS.h" 19 | 20 | bool running = true; 21 | 22 | //set real-time program 23 | void setProcessScheduler(){ 24 | pid_t pid = getpid(); 25 | sched_param param; 26 | param.sched_priority = sched_get_priority_max(SCHED_FIFO); 27 | if (sched_setscheduler(pid, SCHED_FIFO, ¶m) == -1) { 28 | // std::cout << "[ERROR] Function setProcessScheduler failed." << std::endl; 29 | } 30 | } 31 | 32 | int main(int argc, char **argv){ 33 | /* set real-time process */ 34 | setProcessScheduler(); 35 | /* set the print format */ 36 | std::cout << std::fixed << std::setprecision(5); 37 | 38 | 39 | EmptyAction emptyAction((int)ArmFSMStateName::INVALID); 40 | std::vector events; 41 | CtrlComponents *ctrlComp = new CtrlComponents(argc, argv); 42 | 43 | ros::init(argc, argv, "z1_controller"); 44 | 45 | ctrlComp->dt = 1.0/250.; 46 | ctrlComp->armConfigPath = "../config/"; 47 | ctrlComp->stateCSV = new CSVTool("../config/savedArmStates.csv"); 48 | ctrlComp->ioInter = new IOROS(); 49 | ctrlComp->geneObj(); 50 | if(ctrlComp->ctrl == Control::SDK){ 51 | ctrlComp->cmdPanel = new ARMSDK(events, emptyAction, "127.0.0.1", 8072, 8071, 0.002); 52 | }else if(ctrlComp->ctrl == Control::KEYBOARD){ 53 | events.push_back(new StateAction("`", (int)ArmFSMStateName::BACKTOSTART)); 54 | events.push_back(new StateAction("1", (int)ArmFSMStateName::PASSIVE)); 55 | events.push_back(new StateAction("2", (int)ArmFSMStateName::JOINTCTRL)); 56 | events.push_back(new StateAction("3", (int)ArmFSMStateName::CARTESIAN)); 57 | events.push_back(new StateAction("4", (int)ArmFSMStateName::MOVEJ)); 58 | events.push_back(new StateAction("5", (int)ArmFSMStateName::MOVEL)); 59 | events.push_back(new StateAction("6", (int)ArmFSMStateName::MOVEC)); 60 | events.push_back(new StateAction("7", (int)ArmFSMStateName::TEACH)); 61 | events.push_back(new StateAction("8", (int)ArmFSMStateName::TEACHREPEAT)); 62 | events.push_back(new StateAction("9", (int)ArmFSMStateName::SAVESTATE)); 63 | events.push_back(new StateAction("0", (int)ArmFSMStateName::TOSTATE)); 64 | events.push_back(new StateAction("-", (int)ArmFSMStateName::TRAJECTORY)); 65 | events.push_back(new StateAction("=", (int)ArmFSMStateName::CALIBRATION)); 66 | 67 | events.push_back(new ValueAction("q", "a", 0.5)); 68 | events.push_back(new ValueAction("w", "s", 0.5)); 69 | events.push_back(new ValueAction("e", "d", 0.5)); 70 | events.push_back(new ValueAction("r", "f", 0.5)); 71 | events.push_back(new ValueAction("t", "g", 0.5)); 72 | events.push_back(new ValueAction("y", "h", 0.5)); 73 | events.push_back(new ValueAction("down", "up", 1.)); 74 | 75 | ctrlComp->cmdPanel = new Keyboard(events, emptyAction); 76 | } 77 | std::vector states; 78 | states.push_back(new State_Passive(ctrlComp)); 79 | states.push_back(new State_BackToStart(ctrlComp)); 80 | states.push_back(new State_JointSpace(ctrlComp)); 81 | states.push_back(new State_Cartesian(ctrlComp)); 82 | states.push_back(new State_MoveJ(ctrlComp)); 83 | states.push_back(new State_MoveL(ctrlComp)); 84 | states.push_back(new State_MoveC(ctrlComp)); 85 | states.push_back(new State_LowCmd(ctrlComp)); 86 | states.push_back(new State_SaveState(ctrlComp)); 87 | states.push_back(new State_Teach(ctrlComp)); 88 | states.push_back(new State_TeachRepeat(ctrlComp)); 89 | states.push_back(new State_ToState(ctrlComp)); 90 | states.push_back(new State_Trajectory(ctrlComp)); 91 | states.push_back(new State_Calibration(ctrlComp)); 92 | 93 | FiniteStateMachine *fsm; 94 | fsm = new FiniteStateMachine(states, ctrlComp); 95 | 96 | ctrlComp->running = &running; 97 | signal(SIGINT, [](int signum){running = false;}); 98 | std::this_thread::sleep_for(std::chrono::seconds(1)); 99 | ctrlComp->cmdPanel->start(); 100 | while(running){ 101 | std::this_thread::sleep_for(std::chrono::seconds(1)); 102 | } 103 | 104 | delete fsm; 105 | delete ctrlComp; 106 | return 0; 107 | } -------------------------------------------------------------------------------- /unitreeArmTools.py: -------------------------------------------------------------------------------- 1 | # Set the IP address of the z1 robot 2 | # Before running the program, please check the network with the robot 3 | import re 4 | import os 5 | import sys 6 | import socket 7 | import struct 8 | 9 | def check_ip(ipAddr): 10 | compile_ip = re.compile('(((\d{1,2})|(1\d{2})|(2[0-4]\d)|(25[0-5]))\.){3}((\d{1,2})|(1\d{2})|(2[0-4]\d)|(25[0-5]))') 11 | if compile_ip.match(ipAddr): 12 | return True 13 | else: 14 | print("[ERROR] Please input the valid IP address") 15 | return False 16 | 17 | def ping_ip(ip): 18 | cmd = "ping " + str(ip) + " -c 1 -w 1 >/dev/null" #send once, wait 1s 19 | if os.system(cmd): 20 | print("[ERROR]IP: "+ip+" Destination Host Unreachable") 21 | return False 22 | else: 23 | return True 24 | 25 | if __name__ == '__main__': 26 | # 1. check arm valid 27 | if not ping_ip('192.168.123.110'): 28 | sys.exit(1) 29 | 30 | server_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 31 | address = ("0.0.0.0", 8881) 32 | armAddr = ("192.168.123.110", 8880) 33 | server_socket.bind(address) 34 | pack_format = struct.Struct('6B') 35 | 36 | # 2. read IP address input 37 | print("Please enter the IP address to set, which should be input as *.*.*.* ") 38 | ipAddr = '' 39 | while True: 40 | ipAddr = input("IP to set: ") 41 | if check_ip(ipAddr): 42 | break 43 | ipset = re.split('\.', ipAddr) 44 | 45 | # 3. send udp socket to set IP address 46 | send_data = [7, 1, int(ipset[0]), int(ipset[1]), int(ipset[2]), int(ipset[3])] 47 | send_data = pack_format.pack(*send_data) 48 | server_socket.sendto(send_data, armAddr) 49 | data, client = server_socket.recvfrom(6) 50 | data = pack_format.unpack(data) 51 | ipAddr = str(data[2])+'.'+str(data[3])+'.'+str( data[4])+'.'+str(data[5]) 52 | print("Set correctly! Current robot IP is", ipAddr) 53 | print("Please repower the robot and change the IP setting in the z1_controller/config/config.xml") 54 | --------------------------------------------------------------------------------