├── .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