├── .gitignore ├── ABB_RobotStudio ├── ABB_IRB_120 │ ├── Solution_EGM_Joint_Ctrl.rspag │ └── T_ROB_1 │ │ ├── Module1.mod │ │ └── T_ROB_1.pgf └── ABB_IRB_14000 │ ├── Solution_EGM_Joint_Ctrl.rspag │ ├── T_ROB_L │ ├── Module1.mod │ └── T_ROB_L.pgf │ └── T_ROB_R │ ├── Module1.mod │ └── T_ROB_R.pgf ├── Data ├── ABB_IRB_120 │ ├── theta_actual.txt │ └── theta_desired.txt ├── ABB_IRB_14000_L │ ├── theta_actual.txt │ └── theta_desired.txt └── ABB_IRB_14000_R │ ├── theta_actual.txt │ └── theta_desired.txt ├── Evaluation ├── Analysis │ ├── analyse_data_abb_irb_120.py │ ├── analyse_data_abb_irb_14000_L.py │ └── analyse_data_abb_irb_14000_R.py ├── Collection │ ├── collect_abb_irb_120.py │ ├── collect_abb_irb_14000_L.py │ └── collect_abb_irb_14000_R.py └── Control │ ├── ctrl_abb_irb_120.py │ ├── ctrl_abb_irb_14000_L.py │ └── ctrl_abb_irb_14000_R.py ├── Helpers └── PROTO_Converter_Online.txt ├── LICENSE ├── PROTO └── egm.proto ├── README.md ├── images ├── ABB_IRB_120_Env.png ├── ABB_IRB_14000_Env.png ├── Analysis │ ├── ABB_IRB_120.svg │ ├── ABB_IRB_14000_L.svg │ └── ABB_IRB_14000_R.svg └── EGM.png └── src └── Lib ├── EGM ├── Core.py └── egm_pb2.py ├── Trajectory ├── Core.py └── Utilities.py ├── Transformation ├── Core.py └── Utilities │ └── Mathematics.py └── Utilities └── File_IO.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | .DS_Store 3 | **/.DS_Store 4 | -------------------------------------------------------------------------------- /ABB_RobotStudio/ABB_IRB_120/Solution_EGM_Joint_Ctrl.rspag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rparak/ABB_EGM_Python/aac0bf1456b156b70c8dbcee7c57bc424597b13e/ABB_RobotStudio/ABB_IRB_120/Solution_EGM_Joint_Ctrl.rspag -------------------------------------------------------------------------------- /ABB_RobotStudio/ABB_IRB_120/T_ROB_1/Module1.mod: -------------------------------------------------------------------------------- 1 | MODULE Module1 2 | ! ## =========================================================================== ## 3 | ! MIT License 4 | ! Copyright (c) 2023 Roman Parak 5 | ! Permission is hereby granted, free of charge, to any person obtaining a copy 6 | ! of this software and associated documentation files (the "Software"), to deal 7 | ! in the Software without restriction, including without limitation the rights 8 | ! to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | ! copies of the Software, and to permit persons to whom the Software is 10 | ! furnished to do so, subject to the following conditions: 11 | ! The above copyright notice and this permission notice shall be included in all 12 | ! copies or substantial portions of the Software. 13 | ! THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | ! IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | ! FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | ! AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | ! LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | ! OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | ! SOFTWARE. 20 | ! ## =========================================================================== ## 21 | ! Author : Roman Parak 22 | ! Email : Roman.Parak@outlook.com 23 | ! Github : https://github.com/rparak 24 | ! File Name: T_ROB1/Module1.mod 25 | ! ## =========================================================================== ## 26 | 27 | ! Identifier for the EGM correction 28 | LOCAL VAR egmident egm_id; 29 | ! The work object. Base Frame 30 | LOCAL PERS wobjdata egm_wobj := [FALSE, TRUE, "", [[0.0, 0.0, 0.0],[1.0, 0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0],[1.0, 0.0, 0.0, 0.0]]]; 31 | ! Limits for convergence 32 | ! Orientation: +-0.1 degrees 33 | LOCAL CONST egm_minmax egm_condition_orient := [-0.1, 0.1]; 34 | 35 | ! Description: ! 36 | ! Externally Guided motion (EGM): Control - Main Cycle ! 37 | PROC Main() 38 | ! Move to the starting position 39 | MoveAbsJ [[0,0,0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]], v100, fine, tool0\WObj:=wobj0; 40 | 41 | ! EGM Joint Orientation Control 42 | EGM_JOINT_CONTROL; 43 | ENDPROC 44 | 45 | PROC EGM_JOINT_CONTROL() 46 | ! Description: ! 47 | ! Externally Guided motion (EGM) - Joint Control ! 48 | 49 | ! Release the EGM id. 50 | EGMReset egm_id; 51 | 52 | ! Register an EGM id. 53 | EGMGetId egm_id; 54 | 55 | ! Setup the EGM communication. 56 | EGMSetupUC ROB_1, egm_id, "default", "ROB_1", \Joint; 57 | 58 | WHILE TRUE DO 59 | ! Prepare for an EGM communication session. 60 | EGMActJoint egm_id, 61 | \WObj:=egm_wobj, 62 | \J1:=egm_condition_orient 63 | \J2:=egm_condition_orient 64 | \J3:=egm_condition_orient 65 | \J4:=egm_condition_orient 66 | \J5:=egm_condition_orient 67 | \J6:=egm_condition_orient 68 | \LpFilter:=100 69 | \SampleRate:=4 70 | \MaxPosDeviation:=1000 71 | \MaxSpeedDeviation:=1000; 72 | 73 | ! Start the EGM communication session 74 | EGMRunJoint egm_id, EGM_STOP_HOLD, \J1 \J2 \J3 \J4 \J5 \J6 \CondTime:=1.0 \RampInTime:=0.1 \RampOutTime:=0.1 \PosCorrGain:=1.0; 75 | 76 | ! Release the EGM id 77 | !EGMReset egm_id; 78 | ! Wait 2 seconds {No data from EGM sensor} 79 | !WaitTime 2; 80 | ENDWHILE 81 | 82 | ERROR 83 | IF ERRNO = ERR_UDPUC_COMM THEN 84 | TPWrite "Communication timeout: EGM"; 85 | TRYNEXT; 86 | ENDIF 87 | ENDPROC 88 | ENDMODULE -------------------------------------------------------------------------------- /ABB_RobotStudio/ABB_IRB_120/T_ROB_1/T_ROB_1.pgf: -------------------------------------------------------------------------------- 1 | 2 | 3 | Module1.mod 4 | -------------------------------------------------------------------------------- /ABB_RobotStudio/ABB_IRB_14000/Solution_EGM_Joint_Ctrl.rspag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rparak/ABB_EGM_Python/aac0bf1456b156b70c8dbcee7c57bc424597b13e/ABB_RobotStudio/ABB_IRB_14000/Solution_EGM_Joint_Ctrl.rspag -------------------------------------------------------------------------------- /ABB_RobotStudio/ABB_IRB_14000/T_ROB_L/Module1.mod: -------------------------------------------------------------------------------- 1 | MODULE Module1 2 | ! ## =========================================================================== ## 3 | ! MIT License 4 | ! Copyright (c) 2023 Roman Parak 5 | ! Permission is hereby granted, free of charge, to any person obtaining a copy 6 | ! of this software and associated documentation files (the "Software"), to deal 7 | ! in the Software without restriction, including without limitation the rights 8 | ! to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | ! copies of the Software, and to permit persons to whom the Software is 10 | ! furnished to do so, subject to the following conditions: 11 | ! The above copyright notice and this permission notice shall be included in all 12 | ! copies or substantial portions of the Software. 13 | ! THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | ! IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | ! FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | ! AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | ! LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | ! OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | ! SOFTWARE. 20 | ! ## =========================================================================== ## 21 | ! Author : Roman Parak 22 | ! Email : Roman.Parak@outlook.com 23 | ! Github : https://github.com/rparak 24 | ! File Name: T_ROB1/Module1.mod 25 | ! ## =========================================================================== ## 26 | 27 | ! Identifier for the EGM correction 28 | LOCAL VAR egmident egm_id; 29 | ! The work object. Base Frame 30 | LOCAL PERS wobjdata egm_wobj := [FALSE, TRUE, "", [[0.0, 0.0, 0.0],[1.0, 0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0],[1.0, 0.0, 0.0, 0.0]]]; 31 | ! Limits for convergence 32 | ! Orientation: +-0.1 degrees 33 | LOCAL CONST egm_minmax egm_condition_orient := [-0.1, 0.1]; 34 | 35 | ! Description: ! 36 | ! Externally Guided motion (EGM): Control - Main Cycle ! 37 | PROC Main() 38 | ! Move to the starting position 39 | MoveAbsJ [[0,-130,30,0,40,0], [135,9E9,9E9,9E9,9E9,9E9]] \NoEOffs, v100, fine, tool0\WObj:=wobj0; 40 | 41 | ! EGM Joint Orientation Control 42 | EGM_JOINT_CONTROL; 43 | ENDPROC 44 | 45 | PROC EGM_JOINT_CONTROL() 46 | ! Description: ! 47 | ! Externally Guided motion (EGM) - Joint Control ! 48 | 49 | ! Release the EGM id. 50 | EGMReset egm_id; 51 | 52 | ! Register an EGM id. 53 | EGMGetId egm_id; 54 | 55 | ! Setup the EGM communication. 56 | EGMSetupUC ROB_L, egm_id, "default", "ROB_L", \Joint; 57 | 58 | WHILE TRUE DO 59 | ! Prepare for an EGM communication session. 60 | EGMActJoint egm_id, 61 | \WObj:=egm_wobj, 62 | \J1:=egm_condition_orient 63 | \J2:=egm_condition_orient 64 | \J3:=egm_condition_orient 65 | \J4:=egm_condition_orient 66 | \J5:=egm_condition_orient 67 | \J6:=egm_condition_orient 68 | \J7:=egm_condition_orient 69 | \LpFilter:=100 70 | \SampleRate:=4 71 | \MaxPosDeviation:=1000 72 | \MaxSpeedDeviation:=1000; 73 | 74 | ! Start the EGM communication session 75 | EGMRunJoint egm_id, EGM_STOP_HOLD, \J1 \J2 \J3 \J4 \J5 \J6 \J7 \CondTime:=1.0 \RampInTime:=0.1 \RampOutTime:=0.1 \PosCorrGain:=1.0; 76 | 77 | ! Release the EGM id 78 | !EGMReset egm_id; 79 | ! Wait 2 seconds {No data from EGM sensor} 80 | !WaitTime 2; 81 | ENDWHILE 82 | 83 | ERROR 84 | IF ERRNO = ERR_UDPUC_COMM THEN 85 | TPWrite "Communication timeout: EGM"; 86 | TRYNEXT; 87 | ENDIF 88 | ENDPROC 89 | ENDMODULE -------------------------------------------------------------------------------- /ABB_RobotStudio/ABB_IRB_14000/T_ROB_L/T_ROB_L.pgf: -------------------------------------------------------------------------------- 1 | 2 | 3 | Module1.mod 4 | -------------------------------------------------------------------------------- /ABB_RobotStudio/ABB_IRB_14000/T_ROB_R/Module1.mod: -------------------------------------------------------------------------------- 1 | MODULE Module1 2 | ! ## =========================================================================== ## 3 | ! MIT License 4 | ! Copyright (c) 2023 Roman Parak 5 | ! Permission is hereby granted, free of charge, to any person obtaining a copy 6 | ! of this software and associated documentation files (the "Software"), to deal 7 | ! in the Software without restriction, including without limitation the rights 8 | ! to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | ! copies of the Software, and to permit persons to whom the Software is 10 | ! furnished to do so, subject to the following conditions: 11 | ! The above copyright notice and this permission notice shall be included in all 12 | ! copies or substantial portions of the Software. 13 | ! THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | ! IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | ! FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | ! AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | ! LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | ! OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | ! SOFTWARE. 20 | ! ## =========================================================================== ## 21 | ! Author : Roman Parak 22 | ! Email : Roman.Parak@outlook.com 23 | ! Github : https://github.com/rparak 24 | ! File Name: T_ROB1/Module1.mod 25 | ! ## =========================================================================== ## 26 | 27 | ! Identifier for the EGM correction 28 | LOCAL VAR egmident egm_id; 29 | ! The work object. Base Frame 30 | LOCAL PERS wobjdata egm_wobj := [FALSE, TRUE, "", [[0.0, 0.0, 0.0],[1.0, 0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0],[1.0, 0.0, 0.0, 0.0]]]; 31 | ! Limits for convergence 32 | ! Orientation: +-0.1 degrees 33 | LOCAL CONST egm_minmax egm_condition_orient := [-0.1, 0.1]; 34 | 35 | ! Description: ! 36 | ! Externally Guided motion (EGM): Control - Main Cycle ! 37 | PROC Main() 38 | ! Move to the starting position 39 | MoveAbsJ [[0,-130,30,0,40,0], [-135,9E9,9E9,9E9,9E9,9E9]] \NoEOffs, v100, fine, tool0\WObj:=wobj0; 40 | 41 | ! EGM Joint Orientation Control 42 | EGM_JOINT_CONTROL; 43 | ENDPROC 44 | 45 | PROC EGM_JOINT_CONTROL() 46 | ! Description: ! 47 | ! Externally Guided motion (EGM) - Joint Control ! 48 | 49 | ! Release the EGM id. 50 | EGMReset egm_id; 51 | 52 | ! Register an EGM id. 53 | EGMGetId egm_id; 54 | 55 | ! Setup the EGM communication. 56 | EGMSetupUC ROB_R, egm_id, "default", "ROB_R", \Joint; 57 | 58 | WHILE TRUE DO 59 | ! Prepare for an EGM communication session. 60 | EGMActJoint egm_id, 61 | \WObj:=egm_wobj, 62 | \J1:=egm_condition_orient 63 | \J2:=egm_condition_orient 64 | \J3:=egm_condition_orient 65 | \J4:=egm_condition_orient 66 | \J5:=egm_condition_orient 67 | \J6:=egm_condition_orient 68 | \J7:=egm_condition_orient 69 | \LpFilter:=100 70 | \SampleRate:=4 71 | \MaxPosDeviation:=1000 72 | \MaxSpeedDeviation:=1000; 73 | 74 | ! Start the EGM communication session 75 | EGMRunJoint egm_id, EGM_STOP_HOLD, \J1 \J2 \J3 \J4 \J5 \J6 \J7 \CondTime:=1.0 \RampInTime:=0.1 \RampOutTime:=0.1 \PosCorrGain:=1.0; 76 | 77 | ! Release the EGM id 78 | !EGMReset egm_id; 79 | ! Wait 2 seconds {No data from EGM sensor} 80 | !WaitTime 2; 81 | ENDWHILE 82 | 83 | ERROR 84 | IF ERRNO = ERR_UDPUC_COMM THEN 85 | TPWrite "Communication timeout: EGM"; 86 | TRYNEXT; 87 | ENDIF 88 | ENDPROC 89 | ENDMODULE -------------------------------------------------------------------------------- /ABB_RobotStudio/ABB_IRB_14000/T_ROB_R/T_ROB_R.pgf: -------------------------------------------------------------------------------- 1 | 2 | 3 | Module1.mod 4 | -------------------------------------------------------------------------------- /Evaluation/Analysis/analyse_data_abb_irb_120.py: -------------------------------------------------------------------------------- 1 | # System (Default) 2 | import sys 3 | # Add access if it is not in the system path. 4 | if '../../' + 'src' not in sys.path: 5 | sys.path.append('../../' + 'src') 6 | # Numpy (Array computing) [pip3 install numpy] 7 | import numpy as np 8 | # OS (Operating system interfaces) 9 | import os 10 | # SciencePlots (Matplotlib styles for scientific plotting) [pip3 install SciencePlots] 11 | import scienceplots 12 | # Matplotlib (Visualization) [pip3 install matplotlib] 13 | import matplotlib.pyplot as plt 14 | # Custom Lib.: 15 | # ../Lib/Utilities/File_IO 16 | import Lib.Utilities.File_IO as File_IO 17 | 18 | def main(): 19 | """ 20 | Description: 21 | A program to analyze the difference between the actual and desired position of the robot's joints. 22 | """ 23 | 24 | # Locate the path to the project folder. 25 | project_folder = os.getcwd().split('ABB_EGM_Python')[0] + 'ABB_EGM_Python' 26 | 27 | # Load data from the file. 28 | theta_desired = File_IO.Load(f'{project_folder}/Data/ABB_IRB_120/theta_desired', 'txt', ',') 29 | theta_actual = File_IO.Load(f'{project_folder}/Data/ABB_IRB_120/theta_actual', 'txt', ',') 30 | 31 | # Set the parameters for the scientific style. 32 | plt.style.use(['science']) 33 | 34 | # Create a figure. 35 | fig, ax = plt.subplots(nrows=len(theta_desired[0]), ncols=1) 36 | fig.subplots_adjust(hspace=0.50) 37 | fig.suptitle("Analysis of the difference between the actual and desired position of the robot's joints\nType: ABB IRB 120", y=0.95, fontsize=20) 38 | 39 | n = len(theta_desired[:, 0]); t = np.linspace(0.0, 1.0, n) 40 | for i, ax_i in enumerate(ax): 41 | # Visualization of relevant structures. 42 | ax_i.plot(t, theta_desired[:, i], '-', color='#8ca8c5', linewidth=1.0, markersize = 3.0, 43 | markeredgewidth=1.5, label='Desired Data') 44 | ax_i.plot(t, theta_actual[:, i], '-', color='#ffbf80', linewidth=1.0, markersize = 3.0, 45 | markeredgewidth=1.5, label='Actual Data') 46 | 47 | # Set parameters of the graph (plot). 48 | # Set the x ticks. 49 | ax_i.set_xticks(np.arange(0.0, 1.05, 0.05)) 50 | # Label 51 | ax_i.set_xlabel(r't', fontsize=15, labelpad=10) 52 | ax_i.set_ylabel(r'$\theta_{%d}(t)$' % int(i + 1), fontsize=15, labelpad=10) 53 | # Set parameters of the visualization. 54 | ax_i.grid(which='major', linewidth = 0.75, linestyle = ':') 55 | # Show the labels (legends) of the graph. 56 | ax_i.legend(fontsize=10.0) 57 | 58 | # Show the result. 59 | plt.show() 60 | 61 | if __name__ == '__main__': 62 | sys.exit(main()) -------------------------------------------------------------------------------- /Evaluation/Analysis/analyse_data_abb_irb_14000_L.py: -------------------------------------------------------------------------------- 1 | # System (Default) 2 | import sys 3 | # Add access if it is not in the system path. 4 | if '../../' + 'src' not in sys.path: 5 | sys.path.append('../../' + 'src') 6 | # Numpy (Array computing) [pip3 install numpy] 7 | import numpy as np 8 | # OS (Operating system interfaces) 9 | import os 10 | # SciencePlots (Matplotlib styles for scientific plotting) [pip3 install SciencePlots] 11 | import scienceplots 12 | # Matplotlib (Visualization) [pip3 install matplotlib] 13 | import matplotlib.pyplot as plt 14 | # Custom Lib.: 15 | # ../Lib/Utilities/File_IO 16 | import Lib.Utilities.File_IO as File_IO 17 | 18 | def main(): 19 | """ 20 | Description: 21 | A program to analyze the difference between the actual and desired position of the robot's joints. 22 | """ 23 | 24 | # Locate the path to the project folder. 25 | project_folder = os.getcwd().split('ABB_EGM_Python')[0] + 'ABB_EGM_Python' 26 | 27 | # Load data from the file. 28 | theta_desired = File_IO.Load(f'{project_folder}/Data/ABB_IRB_14000_L/theta_desired', 'txt', ',') 29 | theta_actual = File_IO.Load(f'{project_folder}/Data/ABB_IRB_14000_L/theta_actual', 'txt', ',') 30 | 31 | # Set the parameters for the scientific style. 32 | plt.style.use(['science']) 33 | 34 | # Create a figure. 35 | fig, ax = plt.subplots(nrows=len(theta_desired[0]), ncols=1) 36 | fig.subplots_adjust(hspace=0.60) 37 | fig.suptitle("Analysis of the difference between the actual and desired position of the robot's joints\nType: ABB IRB 14000 (L)", y=0.95, fontsize=20) 38 | 39 | n = len(theta_desired[:, 0]); t = np.linspace(0.0, 1.0, n) 40 | for i, ax_i in enumerate(ax): 41 | # Visualization of relevant structures. 42 | ax_i.plot(t, theta_desired[:, i], '-', color='#8ca8c5', linewidth=1.0, markersize = 3.0, 43 | markeredgewidth=1.5, label='Desired Data') 44 | ax_i.plot(t, theta_actual[:, i], '-', color='#ffbf80', linewidth=1.0, markersize = 3.0, 45 | markeredgewidth=1.5, label='Actual Data') 46 | 47 | # Set parameters of the graph (plot). 48 | # Set the x ticks. 49 | ax_i.set_xticks(np.arange(0.0, 1.05, 0.05)) 50 | # Label 51 | ax_i.set_xlabel(r't', fontsize=15, labelpad=10) 52 | ax_i.set_ylabel(r'$\theta_{%d}(t)$' % int(i + 1), fontsize=15, labelpad=10) 53 | # Set parameters of the visualization. 54 | ax_i.grid(which='major', linewidth = 0.75, linestyle = ':') 55 | # Show the labels (legends) of the graph. 56 | ax_i.legend(fontsize=10.0) 57 | 58 | # Show the result. 59 | plt.show() 60 | 61 | if __name__ == '__main__': 62 | sys.exit(main()) -------------------------------------------------------------------------------- /Evaluation/Analysis/analyse_data_abb_irb_14000_R.py: -------------------------------------------------------------------------------- 1 | # System (Default) 2 | import sys 3 | # Add access if it is not in the system path. 4 | if '../../' + 'src' not in sys.path: 5 | sys.path.append('../../' + 'src') 6 | # Numpy (Array computing) [pip3 install numpy] 7 | import numpy as np 8 | # OS (Operating system interfaces) 9 | import os 10 | # SciencePlots (Matplotlib styles for scientific plotting) [pip3 install SciencePlots] 11 | import scienceplots 12 | # Matplotlib (Visualization) [pip3 install matplotlib] 13 | import matplotlib.pyplot as plt 14 | # Custom Lib.: 15 | # ../Lib/Utilities/File_IO 16 | import Lib.Utilities.File_IO as File_IO 17 | 18 | def main(): 19 | """ 20 | Description: 21 | A program to analyze the difference between the actual and desired position of the robot's joints. 22 | """ 23 | 24 | # Locate the path to the project folder. 25 | project_folder = os.getcwd().split('ABB_EGM_Python')[0] + 'ABB_EGM_Python' 26 | 27 | # Load data from the file. 28 | theta_desired = File_IO.Load(f'{project_folder}/Data/ABB_IRB_14000_R/theta_desired', 'txt', ',') 29 | theta_actual = File_IO.Load(f'{project_folder}/Data/ABB_IRB_14000_R/theta_actual', 'txt', ',') 30 | 31 | # Set the parameters for the scientific style. 32 | plt.style.use(['science']) 33 | 34 | # Create a figure. 35 | fig, ax = plt.subplots(nrows=len(theta_desired[0]), ncols=1) 36 | fig.subplots_adjust(hspace=0.60) 37 | fig.suptitle("Analysis of the difference between the actual and desired position of the robot's joints\nType: ABB IRB 14000 (R)", y=0.95, fontsize=20) 38 | 39 | n = len(theta_desired[:, 0]); t = np.linspace(0.0, 1.0, n) 40 | for i, ax_i in enumerate(ax): 41 | # Visualization of relevant structures. 42 | ax_i.plot(t, theta_desired[:, i], '-', color='#8ca8c5', linewidth=1.0, markersize = 3.0, 43 | markeredgewidth=1.5, label='Desired Data') 44 | ax_i.plot(t, theta_actual[:, i], '-', color='#ffbf80', linewidth=1.0, markersize = 3.0, 45 | markeredgewidth=1.5, label='Actual Data') 46 | 47 | # Set parameters of the graph (plot). 48 | # Set the x ticks. 49 | ax_i.set_xticks(np.arange(0.0, 1.05, 0.05)) 50 | # Label 51 | ax_i.set_xlabel(r't', fontsize=15, labelpad=10) 52 | ax_i.set_ylabel(r'$\theta_{%d}(t)$' % int(i + 1), fontsize=15, labelpad=10) 53 | # Set parameters of the visualization. 54 | ax_i.grid(which='major', linewidth = 0.75, linestyle = ':') 55 | # Show the labels (legends) of the graph. 56 | ax_i.legend(fontsize=10.0) 57 | 58 | # Show the result. 59 | plt.show() 60 | 61 | if __name__ == '__main__': 62 | sys.exit(main()) -------------------------------------------------------------------------------- /Evaluation/Collection/collect_abb_irb_120.py: -------------------------------------------------------------------------------- 1 | # System (Default) 2 | import sys 3 | # Add access if it is not in the system path. 4 | if '../../' + 'src' not in sys.path: 5 | sys.path.append('../../' + 'src') 6 | # Numpy (Array computing) [pip3 install numpy] 7 | import numpy as np 8 | # Time (Time access and conversions) 9 | import time 10 | # OS (Operating system interfaces) 11 | import os 12 | # Custom Lib.: 13 | # ../Lib/EGM/Core 14 | import Lib.EGM.Core 15 | # ../Lib/Transsformation/Utilities/Mathematics 16 | import Lib.Transformation.Utilities.Mathematics as Mathematics 17 | # ../Lib/Trajectory/Utilities 18 | import Lib.Trajectory.Utilities 19 | # ../Lib/Utilities/File_IO 20 | import Lib.Utilities.File_IO as File_IO 21 | 22 | """ 23 | Description: 24 | Initialization of constants. 25 | """ 26 | # EGM time step. 27 | CONST_DT = 0.004 28 | # Simulation stop(t_0), start(t_1) time in seconds. 29 | CONST_T_0 = 0.0 30 | CONST_T_1 = 5.0 31 | 32 | def main(): 33 | """ 34 | Description: 35 | A program to collect data obtained from the robot via EGM. 36 | 37 | Note 1: 38 | The data will be used for the analysis of the precision between the actual 39 | and desired positions of the robot's joints. 40 | 41 | Note 2: 42 | The selected robot is the ABB IRB 120, a six-axis articulated robot. 43 | """ 44 | 45 | theta_desired = []; theta_actual = [] 46 | 47 | # Locate the path to the project folder. 48 | project_folder = os.getcwd().split('ABB_EGM_Python')[0] + 'ABB_EGM_Python' 49 | 50 | # Initialization of the class. 51 | # Start UDP communication. 52 | ABB_IRB_120_EGM_Cls = Lib.EGM.Core.EGM_Control_Cls('127.0.0.1', 6511) 53 | 54 | # Set the actual and desired position of the robot's joints. 55 | theta_0 = Mathematics.Degree_To_Radian(np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0], dtype=np.float64)) 56 | theta_1 = Mathematics.Degree_To_Radian(np.array([50.0, -30.0, 30.0, -40.0, 60.0, -80.0], dtype=np.float64)) 57 | 58 | # Initialization of the class to generate trajectory. 59 | Polynomial_Cls = Lib.Trajectory.Utilities.Polynomial_Profile_Cls(delta_time=CONST_DT) 60 | 61 | # Generation of multi-axis position trajectories from input parameters. 62 | theta_arr = [] 63 | for _, (th_actual, th_desired) in enumerate(zip(theta_0, theta_1)): 64 | (theta_arr_i, _, _) = Polynomial_Cls.Generate(th_actual, th_desired, 0.0, 0.0, 0.0, 0.0, 65 | CONST_T_0, CONST_T_1) 66 | theta_arr.append(theta_arr_i) 67 | 68 | # Move from the starting position to the desired position. 69 | # theta_0 -> theta_1 70 | for _, theta_arr_i in enumerate(np.array(theta_arr, dtype=np.float64).T): 71 | ABB_IRB_120_EGM_Cls.Set_Absolute_Joint_Position(theta_arr_i, False) 72 | 73 | # Store the data. 74 | theta_desired.append(theta_arr_i); theta_actual.append(ABB_IRB_120_EGM_Cls.Theta) 75 | 76 | # Wait the required time. 77 | time.sleep(0.5) 78 | 79 | # Move back from the desired position to the starting position. 80 | # theta_0 <- theta_1 81 | for _, theta_arr_i in enumerate(np.array(theta_arr, dtype=np.float64).T[::-1]): 82 | ABB_IRB_120_EGM_Cls.Set_Absolute_Joint_Position(theta_arr_i, False) 83 | 84 | # Store the data. 85 | theta_desired.append(theta_arr_i); theta_actual.append(ABB_IRB_120_EGM_Cls.Theta) 86 | 87 | # Close UDP communication. 88 | ABB_IRB_120_EGM_Cls.Close() 89 | 90 | # Save the data to a file. 91 | for _, (th_desired_i, th_actual_i) in enumerate(zip(theta_desired, theta_actual)): 92 | File_IO.Save(f'{project_folder}/Data/ABB_IRB_120/theta_desired', th_desired_i.tolist(), 'txt', ',') 93 | File_IO.Save(f'{project_folder}/Data/ABB_IRB_120/theta_actual', th_actual_i.tolist(), 'txt', ',') 94 | 95 | if __name__ == '__main__': 96 | sys.exit(main()) -------------------------------------------------------------------------------- /Evaluation/Collection/collect_abb_irb_14000_L.py: -------------------------------------------------------------------------------- 1 | # System (Default) 2 | import sys 3 | # Add access if it is not in the system path. 4 | if '../../' + 'src' not in sys.path: 5 | sys.path.append('../../' + 'src') 6 | # Numpy (Array computing) [pip3 install numpy] 7 | import numpy as np 8 | # Time (Time access and conversions) 9 | import time 10 | # OS (Operating system interfaces) 11 | import os 12 | # Custom Lib.: 13 | # ../Lib/EGM/Core 14 | import Lib.EGM.Core 15 | # ../Lib/Transsformation/Utilities/Mathematics 16 | import Lib.Transformation.Utilities.Mathematics as Mathematics 17 | # ../Lib/Trajectory/Utilities 18 | import Lib.Trajectory.Utilities 19 | # ../Lib/Utilities/File_IO 20 | import Lib.Utilities.File_IO as File_IO 21 | 22 | """ 23 | Description: 24 | Initialization of constants. 25 | """ 26 | # EGM time step. 27 | CONST_DT = 0.004 28 | # Simulation stop(t_0), start(t_1) time in seconds. 29 | CONST_T_0 = 0.0 30 | CONST_T_1 = 5.0 31 | 32 | def main(): 33 | """ 34 | Description: 35 | A program to collect data obtained from the robot via EGM. 36 | 37 | Note 1: 38 | The data will be used for the analysis of the precision between the actual 39 | and desired positions of the robot's joints. 40 | 41 | Note 2: 42 | The selected robot is the ABB IRB 14000 (left arm), a six-axis articulated robot. 43 | """ 44 | 45 | theta_desired = []; theta_actual = [] 46 | 47 | # Locate the path to the project folder. 48 | project_folder = os.getcwd().split('ABB_EGM_Python')[0] + 'ABB_EGM_Python' 49 | 50 | # Initialization of the class. 51 | # Start UDP communication. 52 | ABB_IRB_14000_L_EGM_Cls = Lib.EGM.Core.EGM_Control_Cls('127.0.0.1', 6511) 53 | 54 | # Set the actual and desired position of the robot's joints. 55 | theta_0 = Mathematics.Degree_To_Radian(np.array([0.0, -130.0, 30.0, 0.0, 40.0, 0.0, 135.0], dtype=np.float64)) 56 | theta_1 = Mathematics.Degree_To_Radian(np.array([-75.0556,-142.5,59.1416,-188.077,103.913,-13.9756,67.7383], dtype=np.float64)) 57 | 58 | # Initialization of the class to generate trajectory. 59 | Polynomial_Cls = Lib.Trajectory.Utilities.Polynomial_Profile_Cls(delta_time=0.004) 60 | 61 | # Generation of multi-axis position trajectories from input parameters. 62 | theta_arr = [] 63 | for _, (th_actual, th_desired) in enumerate(zip(theta_0, theta_1)): 64 | (theta_arr_i, _, _) = Polynomial_Cls.Generate(th_actual, th_desired, 0.0, 0.0, 0.0, 0.0, 65 | CONST_T_0, CONST_T_1) 66 | theta_arr.append(theta_arr_i) 67 | 68 | # Move from the starting position to the desired position. 69 | # theta_0 -> theta_1 70 | for _, theta_arr_i in enumerate(np.array(theta_arr, dtype=np.float64).T): 71 | ABB_IRB_14000_L_EGM_Cls.Set_Absolute_Joint_Position(theta_arr_i, True) 72 | 73 | # Store the data. 74 | theta_desired.append(theta_arr_i); theta_actual.append(ABB_IRB_14000_L_EGM_Cls.Theta) 75 | 76 | # Wait the required time. 77 | time.sleep(0.5) 78 | 79 | # Move back from the desired position to the starting position. 80 | # theta_0 <- theta_1 81 | for _, theta_arr_i in enumerate(np.array(theta_arr, dtype=np.float64).T[::-1]): 82 | ABB_IRB_14000_L_EGM_Cls.Set_Absolute_Joint_Position(theta_arr_i, True) 83 | 84 | # Store the data. 85 | theta_desired.append(theta_arr_i); theta_actual.append(ABB_IRB_14000_L_EGM_Cls.Theta) 86 | 87 | # Close UDP communication. 88 | ABB_IRB_14000_L_EGM_Cls.Close() 89 | 90 | # Save the data to a file. 91 | for _, (th_desired_i, th_actual_i) in enumerate(zip(theta_desired, theta_actual)): 92 | File_IO.Save(f'{project_folder}/Data/ABB_IRB_14000_L/theta_desired', th_desired_i.tolist(), 'txt', ',') 93 | File_IO.Save(f'{project_folder}/Data/ABB_IRB_14000_L/theta_actual', th_actual_i.tolist(), 'txt', ',') 94 | 95 | if __name__ == '__main__': 96 | sys.exit(main()) -------------------------------------------------------------------------------- /Evaluation/Collection/collect_abb_irb_14000_R.py: -------------------------------------------------------------------------------- 1 | # System (Default) 2 | import sys 3 | # Add access if it is not in the system path. 4 | if '../../' + 'src' not in sys.path: 5 | sys.path.append('../../' + 'src') 6 | # Numpy (Array computing) [pip3 install numpy] 7 | import numpy as np 8 | # Time (Time access and conversions) 9 | import time 10 | # OS (Operating system interfaces) 11 | import os 12 | # Custom Lib.: 13 | # ../Lib/EGM/Core 14 | import Lib.EGM.Core 15 | # ../Lib/Transsformation/Utilities/Mathematics 16 | import Lib.Transformation.Utilities.Mathematics as Mathematics 17 | # ../Lib/Trajectory/Utilities 18 | import Lib.Trajectory.Utilities 19 | # ../Lib/Utilities/File_IO 20 | import Lib.Utilities.File_IO as File_IO 21 | 22 | """ 23 | Description: 24 | Initialization of constants. 25 | """ 26 | # EGM time step. 27 | CONST_DT = 0.004 28 | # Simulation stop(t_0), start(t_1) time in seconds. 29 | CONST_T_0 = 0.0 30 | CONST_T_1 = 5.0 31 | 32 | def main(): 33 | """ 34 | Description: 35 | A program to collect data obtained from the robot via EGM. 36 | 37 | Note 1: 38 | The data will be used for the analysis of the precision between the actual 39 | and desired positions of the robot's joints. 40 | 41 | Note 2: 42 | The selected robot is the ABB IRB 14000 (right arm), a six-axis articulated robot. 43 | """ 44 | 45 | theta_desired = []; theta_actual = [] 46 | 47 | # Locate the path to the project folder. 48 | project_folder = os.getcwd().split('ABB_EGM_Python')[0] + 'ABB_EGM_Python' 49 | 50 | # Initialization of the class. 51 | # Start UDP communication. 52 | ABB_IRB_14000_R_EGM_Cls = Lib.EGM.Core.EGM_Control_Cls('127.0.0.1', 6512) 53 | 54 | # Set the actual and desired position of the robot's joints. 55 | theta_0 = Mathematics.Degree_To_Radian(np.array([0.0, -130.0, 30.0, 0.0, 40.0, 0.0, -135.0], dtype=np.float64)) 56 | theta_1 = Mathematics.Degree_To_Radian(np.array([75.0556,-142.5,59.1416,-171.923,103.913,13.9756,-67.7383], dtype=np.float64)) 57 | 58 | # Initialization of the class to generate trajectory. 59 | Polynomial_Cls = Lib.Trajectory.Utilities.Polynomial_Profile_Cls(delta_time=0.004) 60 | 61 | # Generation of multi-axis position trajectories from input parameters. 62 | theta_arr = [] 63 | for _, (th_actual, th_desired) in enumerate(zip(theta_0, theta_1)): 64 | (theta_arr_i, _, _) = Polynomial_Cls.Generate(th_actual, th_desired, 0.0, 0.0, 0.0, 0.0, 65 | CONST_T_0, CONST_T_1) 66 | theta_arr.append(theta_arr_i) 67 | 68 | # Move from the starting position to the desired position. 69 | # theta_0 -> theta_1 70 | for _, theta_arr_i in enumerate(np.array(theta_arr, dtype=np.float64).T): 71 | ABB_IRB_14000_R_EGM_Cls.Set_Absolute_Joint_Position(theta_arr_i, True) 72 | 73 | # Store the data. 74 | theta_desired.append(theta_arr_i); theta_actual.append(ABB_IRB_14000_R_EGM_Cls.Theta) 75 | 76 | # Wait the required time. 77 | time.sleep(0.5) 78 | 79 | # Move back from the desired position to the starting position. 80 | # theta_0 <- theta_1 81 | for _, theta_arr_i in enumerate(np.array(theta_arr, dtype=np.float64).T[::-1]): 82 | ABB_IRB_14000_R_EGM_Cls.Set_Absolute_Joint_Position(theta_arr_i, True) 83 | 84 | # Store the data. 85 | theta_desired.append(theta_arr_i); theta_actual.append(ABB_IRB_14000_R_EGM_Cls.Theta) 86 | 87 | # Close UDP communication. 88 | ABB_IRB_14000_R_EGM_Cls.Close() 89 | 90 | # Save the data to a file. 91 | for _, (th_desired_i, th_actual_i) in enumerate(zip(theta_desired, theta_actual)): 92 | File_IO.Save(f'{project_folder}/Data/ABB_IRB_14000_R/theta_desired', th_desired_i.tolist(), 'txt', ',') 93 | File_IO.Save(f'{project_folder}/Data/ABB_IRB_14000_R/theta_actual', th_actual_i.tolist(), 'txt', ',') 94 | 95 | if __name__ == '__main__': 96 | sys.exit(main()) -------------------------------------------------------------------------------- /Evaluation/Control/ctrl_abb_irb_120.py: -------------------------------------------------------------------------------- 1 | # System (Default) 2 | import sys 3 | # Add access if it is not in the system path. 4 | if '../../' + 'src' not in sys.path: 5 | sys.path.append('../../' + 'src') 6 | # Numpy (Array computing) [pip3 install numpy] 7 | import numpy as np 8 | # Time (Time access and conversions) 9 | import time 10 | # Custom Lib.: 11 | # ../Lib/EGM/Core 12 | import Lib.EGM.Core 13 | # ../Lib/Transsformation/Utilities/Mathematics 14 | import Lib.Transformation.Utilities.Mathematics as Mathematics 15 | # ../Lib/Trajectory/Utilities 16 | import Lib.Trajectory.Utilities 17 | 18 | """ 19 | Description: 20 | Initialization of constants. 21 | """ 22 | # EGM time step. 23 | CONST_DT = 0.004 24 | # Simulation stop(t_0), start(t_1) time in seconds. 25 | CONST_T_0 = 0.0 26 | CONST_T_1 = 5.0 27 | 28 | def main(): 29 | """ 30 | Description: 31 | A program to test the control of an articulated robotic arm using EGM. 32 | 33 | Note: 34 | The selected robot is the ABB IRB 120, a six-axis articulated robot. 35 | """ 36 | 37 | # Initialization of the class. 38 | # Start UDP communication. 39 | ABB_IRB_120_EGM_Cls = Lib.EGM.Core.EGM_Control_Cls('127.0.0.1', 6511) 40 | 41 | # Set the actual and desired position of the robot's joints. 42 | theta_0 = Mathematics.Degree_To_Radian(np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0], dtype=np.float64)) 43 | theta_1 = Mathematics.Degree_To_Radian(np.array([50.0, -30.0, 30.0, -40.0, 60.0, -80.0], dtype=np.float64)) 44 | 45 | # Initialization of the class to generate trajectory. 46 | Polynomial_Cls = Lib.Trajectory.Utilities.Polynomial_Profile_Cls(delta_time=CONST_DT) 47 | 48 | # Generation of multi-axis position trajectories from input parameters. 49 | theta_arr = [] 50 | for _, (th_actual, th_desired) in enumerate(zip(theta_0, theta_1)): 51 | (theta_arr_i, _, _) = Polynomial_Cls.Generate(th_actual, th_desired, 0.0, 0.0, 0.0, 0.0, 52 | CONST_T_0, CONST_T_1) 53 | theta_arr.append(theta_arr_i) 54 | 55 | # Move from the starting position to the desired position. 56 | # theta_0 -> theta_1 57 | for _, theta_arr_i in enumerate(np.array(theta_arr, dtype=np.float64).T): 58 | ABB_IRB_120_EGM_Cls.Set_Absolute_Joint_Position(theta_arr_i, False) 59 | 60 | # Wait the required time. 61 | time.sleep(0.5) 62 | 63 | # Move back from the desired position to the starting position. 64 | # theta_0 <- theta_1 65 | for _, theta_arr_i in enumerate(np.array(theta_arr, dtype=np.float64).T[::-1]): 66 | ABB_IRB_120_EGM_Cls.Set_Absolute_Joint_Position(theta_arr_i, False) 67 | 68 | # Close UDP communication. 69 | ABB_IRB_120_EGM_Cls.Close() 70 | 71 | if __name__ == '__main__': 72 | sys.exit(main()) -------------------------------------------------------------------------------- /Evaluation/Control/ctrl_abb_irb_14000_L.py: -------------------------------------------------------------------------------- 1 | # System (Default) 2 | import sys 3 | # Add access if it is not in the system path. 4 | if '../../' + 'src' not in sys.path: 5 | sys.path.append('../../' + 'src') 6 | # Numpy (Array computing) [pip3 install numpy] 7 | import numpy as np 8 | # Time (Time access and conversions) 9 | import time 10 | # Custom Lib.: 11 | # ../Lib/EGM/Core 12 | import Lib.EGM.Core 13 | # ../Lib/Transsformation/Utilities/Mathematics 14 | import Lib.Transformation.Utilities.Mathematics as Mathematics 15 | # ../Lib/Trajectory/Utilities 16 | import Lib.Trajectory.Utilities 17 | 18 | """ 19 | Description: 20 | Initialization of constants. 21 | """ 22 | # EGM time step. 23 | CONST_DT = 0.004 24 | # Simulation stop(t_0), start(t_1) time in seconds. 25 | CONST_T_0 = 0.0 26 | CONST_T_1 = 5.0 27 | 28 | def main(): 29 | """ 30 | Description: 31 | A program to test the control of an articulated robotic arm using EGM. 32 | 33 | Note: 34 | The selected robot is the ABB IRB 14000 (left arm), a six-axis articulated robot. 35 | """ 36 | 37 | # Initialization of the class. 38 | # Start UDP communication. 39 | ABB_IRB_14000_L_EGM_Cls = Lib.EGM.Core.EGM_Control_Cls('127.0.0.1', 6511) 40 | 41 | # Set the actual and desired position of the robot's joints. 42 | theta_0 = Mathematics.Degree_To_Radian(np.array([0.0, -130.0, 30.0, 0.0, 40.0, 0.0, 135.0], dtype=np.float64)) 43 | theta_1 = Mathematics.Degree_To_Radian(np.array([-75.0556,-142.5,59.1416,-188.077,103.913,-13.9756,67.7383], dtype=np.float64)) 44 | 45 | # Initialization of the class to generate trajectory. 46 | Polynomial_Cls = Lib.Trajectory.Utilities.Polynomial_Profile_Cls(delta_time=0.004) 47 | 48 | # Generation of multi-axis position trajectories from input parameters. 49 | theta_arr = [] 50 | for _, (th_actual, th_desired) in enumerate(zip(theta_0, theta_1)): 51 | (theta_arr_i, _, _) = Polynomial_Cls.Generate(th_actual, th_desired, 0.0, 0.0, 0.0, 0.0, 52 | CONST_T_0, CONST_T_1) 53 | theta_arr.append(theta_arr_i) 54 | 55 | # Move from the starting position to the desired position. 56 | # theta_0 -> theta_1 57 | for _, theta_arr_i in enumerate(np.array(theta_arr, dtype=np.float64).T): 58 | ABB_IRB_14000_L_EGM_Cls.Set_Absolute_Joint_Position(theta_arr_i, True) 59 | 60 | # Wait the required time. 61 | time.sleep(0.5) 62 | 63 | # Move back from the desired position to the starting position. 64 | # theta_0 <- theta_1 65 | for _, theta_arr_i in enumerate(np.array(theta_arr, dtype=np.float64).T[::-1]): 66 | ABB_IRB_14000_L_EGM_Cls.Set_Absolute_Joint_Position(theta_arr_i, True) 67 | 68 | # Close UDP communication. 69 | ABB_IRB_14000_L_EGM_Cls.Close() 70 | 71 | if __name__ == '__main__': 72 | sys.exit(main()) -------------------------------------------------------------------------------- /Evaluation/Control/ctrl_abb_irb_14000_R.py: -------------------------------------------------------------------------------- 1 | # System (Default) 2 | import sys 3 | # Add access if it is not in the system path. 4 | if '../../' + 'src' not in sys.path: 5 | sys.path.append('../../' + 'src') 6 | # Numpy (Array computing) [pip3 install numpy] 7 | import numpy as np 8 | # Time (Time access and conversions) 9 | import time 10 | # Custom Lib.: 11 | # ../Lib/EGM/Core 12 | import Lib.EGM.Core 13 | # ../Lib/Transsformation/Utilities/Mathematics 14 | import Lib.Transformation.Utilities.Mathematics as Mathematics 15 | # ../Lib/Trajectory/Utilities 16 | import Lib.Trajectory.Utilities 17 | 18 | """ 19 | Description: 20 | Initialization of constants. 21 | """ 22 | # EGM time step. 23 | CONST_DT = 0.004 24 | # Simulation stop(t_0), start(t_1) time in seconds. 25 | CONST_T_0 = 0.0 26 | CONST_T_1 = 5.0 27 | 28 | def main(): 29 | """ 30 | Description: 31 | A program to test the control of an articulated robotic arm using EGM. 32 | 33 | Note: 34 | The selected robot is the ABB IRB 14000 (right arm), a six-axis articulated robot. 35 | """ 36 | 37 | # Initialization of the class. 38 | # Start UDP communication. 39 | ABB_IRB_14000_R_EGM_Cls = Lib.EGM.Core.EGM_Control_Cls('127.0.0.1', 6512) 40 | 41 | # Set the actual and desired position of the robot's joints. 42 | theta_0 = Mathematics.Degree_To_Radian(np.array([0.0, -130.0, 30.0, 0.0, 40.0, 0.0, -135.0], dtype=np.float64)) 43 | theta_1 = Mathematics.Degree_To_Radian(np.array([75.0556,-142.5,59.1416,-171.923,103.913,13.9756,-67.7383], dtype=np.float64)) 44 | 45 | # Initialization of the class to generate trajectory. 46 | Polynomial_Cls = Lib.Trajectory.Utilities.Polynomial_Profile_Cls(delta_time=0.004) 47 | 48 | # Generation of multi-axis position trajectories from input parameters. 49 | theta_arr = [] 50 | for _, (th_actual, th_desired) in enumerate(zip(theta_0, theta_1)): 51 | (theta_arr_i, _, _) = Polynomial_Cls.Generate(th_actual, th_desired, 0.0, 0.0, 0.0, 0.0, 52 | CONST_T_0, CONST_T_1) 53 | theta_arr.append(theta_arr_i) 54 | 55 | # Move from the starting position to the desired position. 56 | # theta_0 -> theta_1 57 | for _, theta_arr_i in enumerate(np.array(theta_arr, dtype=np.float64).T): 58 | ABB_IRB_14000_R_EGM_Cls.Set_Absolute_Joint_Position(theta_arr_i, True) 59 | 60 | # Wait the required time. 61 | time.sleep(0.5) 62 | 63 | # Move back from the desired position to the starting position. 64 | # theta_0 <- theta_1 65 | for _, theta_arr_i in enumerate(np.array(theta_arr, dtype=np.float64).T[::-1]): 66 | ABB_IRB_14000_R_EGM_Cls.Set_Absolute_Joint_Position(theta_arr_i, True) 67 | 68 | # Close UDP communication. 69 | ABB_IRB_14000_R_EGM_Cls.Close() 70 | 71 | if __name__ == '__main__': 72 | sys.exit(main()) -------------------------------------------------------------------------------- /Helpers/PROTO_Converter_Online.txt: -------------------------------------------------------------------------------- 1 | https://protogen.marcgravell.com -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Roman Parak 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /PROTO/egm.proto: -------------------------------------------------------------------------------- 1 | // Definition of ABB sensor interface V1.1 2 | // 3 | // messages of type EgmRobot are sent out from the robot controller 4 | // messages of type EgmSensor are sent to the robot controller 5 | // 6 | package abb.egm; 7 | 8 | message EgmHeader 9 | { 10 | optional uint32 seqno = 1; // sequence number (to be able to find lost messages) 11 | optional uint32 tm = 2; // controller send time stamp in ms 12 | 13 | enum MessageType { 14 | MSGTYPE_UNDEFINED = 0; 15 | MSGTYPE_COMMAND = 1; // for future use 16 | MSGTYPE_DATA = 2; // sent by robot controller 17 | MSGTYPE_CORRECTION = 3; // sent by sensor for position guidance 18 | MSGTYPE_PATH_CORRECTION = 4; // sent by sensor for path correction 19 | } 20 | 21 | optional MessageType mtype = 3 [default = MSGTYPE_UNDEFINED]; 22 | } 23 | 24 | message EgmCartesian // Cartesian position in mm 25 | { 26 | required double x = 1; 27 | required double y = 2; 28 | required double z = 3; 29 | } 30 | 31 | // If you have pose input, i.e. not joint input, you can choose to send orientation data as quaternion or as Euler angles. 32 | // If both are sent, Euler angles have higher priority. 33 | 34 | message EgmQuaternion // Quaternion orientation 35 | { 36 | required double u0 = 1; 37 | required double u1 = 2; 38 | required double u2 = 3; 39 | required double u3 = 4; 40 | } 41 | 42 | message EgmEuler // Euler angle orientation in degrees 43 | { 44 | required double x = 1; 45 | required double y = 2; 46 | required double z = 3; 47 | } 48 | 49 | message EgmClock // Time in seconds and microseconds since 1 Jan 1970 50 | { 51 | required uint64 sec = 1; 52 | required uint64 usec = 2; 53 | } 54 | 55 | message EgmPose // Pose (i.e. cartesian position and Quaternion orientation) relative to the correction frame defined by EGMActPose 56 | { 57 | optional EgmCartesian pos = 1; 58 | optional EgmQuaternion orient = 2; 59 | optional EgmEuler euler = 3; 60 | } 61 | 62 | message EgmCartesianSpeed // Array of 6 speed reference values in mm/s or degrees/s 63 | { 64 | repeated double value = 1; 65 | } 66 | 67 | message EgmJoints // Array of 6 joint values for TCP robot in degrees 68 | { 69 | repeated double joints = 1; 70 | } 71 | 72 | message EgmExternalJoints // Array of 6 joint values for additional axis, in degrees for rotating axis, in mm for linear axis 73 | { 74 | repeated double joints = 1; 75 | } 76 | 77 | message EgmPlanned // Planned position for robot (joints or cartesian) and additional axis (array of 6 values) 78 | { // Is used for position streaming (source: controller) and position guidance (source: sensor) 79 | optional EgmJoints joints = 1; 80 | optional EgmPose cartesian = 2; 81 | optional EgmJoints externalJoints = 3; 82 | optional EgmClock time = 4; 83 | } 84 | 85 | message EgmSpeedRef // Speed reference values for robot (joint or cartesian) and additional axis (array of 6 values) 86 | { 87 | optional EgmJoints joints = 1; 88 | optional EgmCartesianSpeed cartesians = 2; 89 | optional EgmJoints externalJoints = 3; 90 | } 91 | 92 | 93 | message EgmPathCorr // Cartesian path correction and measurment age 94 | { 95 | required EgmCartesian pos = 1; // Sensor measurement (x, y, z) relative the sensor tool coordinate system 96 | required uint32 age = 2; // Sensor measurement age in ms 97 | } 98 | 99 | 100 | message EgmFeedBack // Feed back position, i.e. actual measured position for robot (joints or cartesian) and additional axis (array of 6 values) 101 | { 102 | optional EgmJoints joints = 1; 103 | optional EgmPose cartesian = 2; 104 | optional EgmJoints externalJoints = 3; 105 | optional EgmClock time = 4; 106 | } 107 | 108 | message EgmMotorState // Motor state 109 | { 110 | enum MotorStateType { 111 | MOTORS_UNDEFINED = 0; 112 | MOTORS_ON = 1; 113 | MOTORS_OFF = 2; 114 | } 115 | 116 | required MotorStateType state = 1; 117 | } 118 | 119 | message EgmMCIState // EGM state 120 | { 121 | enum MCIStateType { 122 | MCI_UNDEFINED = 0; 123 | MCI_ERROR = 1; 124 | MCI_STOPPED = 2; 125 | MCI_RUNNING = 3; 126 | } 127 | 128 | required MCIStateType state = 1 [default = MCI_UNDEFINED]; 129 | } 130 | 131 | message EgmRapidCtrlExecState // RAPID execution state 132 | { 133 | enum RapidCtrlExecStateType { 134 | RAPID_UNDEFINED = 0; 135 | RAPID_STOPPED = 1; 136 | RAPID_RUNNING = 2; 137 | }; 138 | 139 | required RapidCtrlExecStateType state = 1 [default = RAPID_UNDEFINED]; 140 | } 141 | 142 | message EgmTestSignals // Test signals 143 | { 144 | repeated double signals = 1; 145 | } 146 | 147 | message EgmMeasuredForce // Array of 6 force values for a robot 148 | { 149 | repeated double force = 1; 150 | } 151 | 152 | // Robot controller outbound message, sent from the controller to the sensor during position guidance and position streaming 153 | message EgmRobot 154 | { 155 | optional EgmHeader header = 1; 156 | optional EgmFeedBack feedBack = 2; 157 | optional EgmPlanned planned = 3; 158 | 159 | optional EgmMotorState motorState = 4; 160 | optional EgmMCIState mciState = 5; 161 | optional bool mciConvergenceMet = 6; 162 | optional EgmTestSignals testSignals = 7; 163 | optional EgmRapidCtrlExecState rapidExecState = 8; 164 | optional EgmMeasuredForce measuredForce = 9; 165 | optional double utilizationRate=10; 166 | } 167 | 168 | 169 | // Robot controller inbound message, sent from sensor to the controller during position guidance 170 | message EgmSensor 171 | { 172 | optional EgmHeader header = 1; 173 | optional EgmPlanned planned = 2; 174 | optional EgmSpeedRef speedRef = 3; 175 | } 176 | 177 | // Robot controller inbound message, sent from sensor during path correction 178 | message EgmSensorPathCorr 179 | { 180 | optional EgmHeader header = 1; 181 | optional EgmPathCorr pathCorr = 2; 182 | } 183 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # An Open-Source Python Library to Control ABB Robot Arm via EGM 2 | 3 |

4 | 5 |

6 | 7 | ## Requirements 8 | 9 | **Software:** 10 | ```bash 11 | ABB RobotStudio 12 | ``` 13 | 14 | **Programming Language** 15 | 16 | ```bash 17 | Python 18 | ``` 19 | 20 | **Import Libraries** 21 | ```bash 22 | More information can be found in the individual scripts (*.py). 23 | ``` 24 | 25 | **Supported on the following operating systems:** 26 | ```bash 27 | Windows 28 | ``` 29 | 30 | | Software/Package | Link | 31 | | --------------------- | ------------------------------------------------------------------------------------- | 32 | | ABB RobotStudio | https://new.abb.com/products/robotics/robotstudio/downloads | 33 | 34 | ## Project Description 35 | 36 | An open-source Python library designed for controlling an articulated robotic arm from ABB through Externally Guided Motion (EGM). Two types of robots from ABB were chosen for testing, namely the industrial robotic arm IRB 120 and the collaborative robotic arm IRB 14000 (YuMi). The goal was to analyze the difference between the actual and desired positions of the robot's joints generated using polynomial multi-axis position trajectories. 37 | 38 | The project also includes the collection of data using the EGM, as well as the analysis of the data. 39 | 40 | The project was realized at the Institute of Automation and Computer Science, Brno University of Technology, Faculty of Mechanical Engineering (NETME Centre - Cybernetics and Robotics Division). 41 | 42 | **IP Address Settings:** 43 | 44 | | | ABB RobotStudio | PC | 45 | | :------: | :-----------: | :-----------: | 46 | | Simulation Control | 127.0.0.1 | 127.0.0.1 | 47 | 48 | | | PORT | 49 | | :------: | :-----------: | 50 | | UDPUC | 6511 | 51 | | UDPUC | 6512 | 52 | 53 | Both ports are only used when the YuMi robot is selected, as each arm can be controlled separately. 54 | 55 | **WARNING: To control the robot in the real world, it is necessary to disable the firewall.** 56 | 57 |

58 | 59 | 60 |

61 | 62 | **Notes:** 63 | 64 | EGM (Externally Guided Motion) is an interface for ABB robots that allows smoothless control of the robotic arm from an external application. The EGM can be used to transfer positions to the robot controller in either Joint/ Cartesian space. In our case it is the control of the robot using Joint coordinates. 65 | 66 | ```bash 67 | The file "egm.proto" can be found in the installation folder of RobotWare. For example on Windows with RobotWare 7.6.1: 68 | C:\Users\\AppData\Local\ABB Industrial IT\Robotics IT\RobotWare\RobotControl_7.6.1\utility\Template\EGM 69 | ``` 70 | 71 | The Protobuf code generator can be used to generate code from a *.proto file into individual programming languages. 72 | 73 | Link: [Protobuf Code Generator and Parser](https://protogen.marcgravell.com) 74 | 75 | ## Project Hierarchy 76 | 77 | ```bash 78 | [/ABB_RobotStudio/{ABB_IRB_120, ABB_IRB_14000}/../] 79 | Description: 80 | The main ABB RobotStudio project for robot control via EGM. 81 | 82 | [/src/Lib/EGM/egm_pb2.py] 83 | Description: 84 | Autogenerated code from the egm.proto -file. 85 | 86 | [/src/Lib/EGM/Core.py] 87 | Description: 88 | A class to control an articulated robotic arm through the use of Externally Guided Motion (EGM) within ABB RobotStudio. 89 | 90 | [/Data/../] 91 | Description: 92 | The folder contains the collected data from each tested robotic arm. 93 | 94 | [/Evaluation/{Control, Collection, Analysis}/] 95 | Description: 96 | The evaluation (analysis) of the designed class. 97 | 98 | [/PROTO/] 99 | Description: 100 | The egm.proto -file. 101 | ``` 102 | 103 | ## Data Evaluation 104 | 105 | The results of the analysis show the difference between the actual and desired positions of the robot's joints for each tested robotic arm. 106 | 107 | **ABB IRB 120** 108 | 109 |

110 | 111 |

112 | 113 | **ABB IRB 14000 (L)** 114 | 115 |

116 | 117 |

118 | 119 | **ABB IRB 14000 (R)** 120 | 121 |

122 | 123 |

124 | 125 | ## Contact Info 126 | Roman.Parak@outlook.com 127 | 128 | ## Citation (BibTex) 129 | ```bash 130 | @misc{RomanParak_PyEGM, 131 | author = {Roman Parak}, 132 | title = {An open-source python library to control abb robot arm via egm}, 133 | year = {2023}, 134 | publisher = {GitHub}, 135 | journal = {GitHub repository}, 136 | howpublished = {\url{https://https://github.com/rparak/Transformation}} 137 | } 138 | ``` 139 | 140 | ## License 141 | [MIT](https://choosealicense.com/licenses/mit/) 142 | -------------------------------------------------------------------------------- /images/ABB_IRB_120_Env.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rparak/ABB_EGM_Python/aac0bf1456b156b70c8dbcee7c57bc424597b13e/images/ABB_IRB_120_Env.png -------------------------------------------------------------------------------- /images/ABB_IRB_14000_Env.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rparak/ABB_EGM_Python/aac0bf1456b156b70c8dbcee7c57bc424597b13e/images/ABB_IRB_14000_Env.png -------------------------------------------------------------------------------- /images/EGM.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rparak/ABB_EGM_Python/aac0bf1456b156b70c8dbcee7c57bc424597b13e/images/EGM.png -------------------------------------------------------------------------------- /src/Lib/EGM/Core.py: -------------------------------------------------------------------------------- 1 | """ 2 | ## =========================================================================== ## 3 | MIT License 4 | Copyright (c) 2023 Roman Parak 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | The above copyright notice and this permission notice shall be included in all 12 | copies or substantial portions of the Software. 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. 20 | ## =========================================================================== ## 21 | Author : Roman Parak 22 | Email : Roman.Parak@outlook.com 23 | Github : https://github.com/rparak 24 | File Name: Core.py 25 | ## =========================================================================== ## 26 | """ 27 | 28 | # Socket (Low-level networking interface) 29 | import socket 30 | # Egm_pb2 (Library generated by the EGM proto file) 31 | # Note: 32 | # The protobuf library needs to be installed. 33 | # pip install protobuf==3.20.3 34 | import Lib.EGM.egm_pb2 as egm_pb2 35 | # Numpy (Array computing) [pip3 install numpy] 36 | import numpy as np 37 | # Typing (Support for type hints) 38 | import typing as tp 39 | # Custom Lib.: 40 | # ../Lib/Transformation/Core 41 | from Lib.Transformation.Core import Homogeneous_Transformation_Matrix_Cls as HTM_Cls 42 | # ../Lib/Transsformation/Utilities/Mathematics 43 | import Lib.Transformation.Utilities.Mathematics as Mathematics 44 | 45 | """ 46 | Description: 47 | - EGM (Externally Guided Motion) is an interface for ABB robots that allows smoothness control of the robot 48 | arm from an external application. 49 | - EGM can also be used to stream current positions from the robot in either joint/cartesian 50 | space. 51 | - The application communicates with the ABB robot via UDP (User Datagram Protocol). 52 | 53 | Note: 54 | For more detailed information on EGMs, please refer to the application guide on the ABB website. 55 | """ 56 | 57 | """ 58 | Description: 59 | Initialization of constants. 60 | """ 61 | CONST_BUFSIZE = 2**16 62 | 63 | class EGM_Control_Cls(object): 64 | """ 65 | Description: 66 | A class to control an articulated robotic arm through the use of Externally Guided 67 | Motion (EGM) within ABB RobotStudio. 68 | 69 | Note: 70 | The IP address and port number of the UDP server will be set 71 | in the ABB RobotStudio settings. 72 | 73 | Initialization of the Class: 74 | Args: 75 | (1) id_address [string]: The IP address of the UDP server. 76 | (2) port_number [INT]: The port number of the UDP server. 77 | 78 | Example: 79 | Initialization: 80 | # Assignment of the variables. 81 | id_address = '127.0.0.1' 82 | port_number = 6511 83 | 84 | # Initialization of the class. 85 | Cls = EGM_Control_Cls(id_address, port_number) 86 | 87 | Features: 88 | # Properties of the class. 89 | Cls.Time 90 | ... 91 | Cls.Theta, Cls.T_EE 92 | 93 | # Functions of the class. 94 | Cls.Set_Absolute_Joint_Position() 95 | """ 96 | 97 | def __init__(self, id_address: str = '127.0.0.1', port_number: int = 6511): 98 | try: 99 | # Create a UDP client to communicate with the ABB RobotStudio 100 | # application. 101 | self.__udp_client = socket.socket(socket.AF_INET, 102 | socket.SOCK_DGRAM) 103 | self.__udp_client.bind((id_address, port_number)) 104 | except socket.error as e: 105 | print(f'[ERROR] Information: {e}') 106 | print('[ERROR] An error occurred while creating the socket. The IP address or port was incorrectly entered.') 107 | 108 | # The structure of the sensor message for reading the data. 109 | self.__sensor_message_r = egm_pb2.EgmRobot() 110 | 111 | # The data to be obtained (streamed) from the robot. 112 | # The actual machine time (Timestamp) in milliseconds. 113 | self.__t = 0.0 114 | # Homogeneous transformation matrix of the End-Effector. 115 | self.__T_EE = HTM_Cls(None, np.float64) 116 | # Absolute position of the joint in radians. 117 | self.__theta = [] 118 | 119 | @property 120 | def Time(self) -> float: 121 | """ 122 | Description: 123 | Get the actual machine time (Timestamp) in milliseconds. 124 | 125 | Note: 126 | The time is set within a software ABB RobotStudio. 127 | See function "EGMActJoint". 128 | \SampleRate:={value in ms} 129 | 130 | Returns: 131 | (1) parameter [float]: The actual machine time in milliseconds. 132 | """ 133 | 134 | return self.__t 135 | 136 | @property 137 | def Theta(self) -> tp.List[float]: 138 | """ 139 | Description: 140 | Get the absolute positions of the robot's joints. 141 | 142 | Returns: 143 | (1) parameter [Vector 1xn]: Current absolute joint position in radians. 144 | Note: 145 | Where n is the number of joints. 146 | """ 147 | 148 | return Mathematics.Degree_To_Radian(self.__theta) 149 | 150 | @property 151 | def T_EE(self) -> tp.List[tp.List[float]]: 152 | """ 153 | Description: 154 | Get the homogeneous transformation matrix of the robot end-effector. 155 | 156 | Returns: 157 | (1) parameter [Matrix 4x4]: Homogeneous transformation matrix of the End-Effector. 158 | """ 159 | 160 | return self.__T_EE 161 | 162 | def Close(self) -> None: 163 | """ 164 | Description: 165 | A function to close UDP communication. 166 | """ 167 | 168 | self.__udp_client.close() 169 | 170 | def __Send_Sensor_Message(self, theta: tp.List[float], enable_external_joint: bool, end_point: tp.Tuple[str, int]) -> None: 171 | """ 172 | Description: 173 | Function to send data to the robot. 174 | 175 | Args: 176 | (1) theta [Vector 1xn]: Desired absolute joint position in radians. 177 | Note: 178 | Where n is the number of joints. 179 | (2) enable_external_joint [bool]: Information about whether or not the robot has an external joint. 180 | (3) end_point [Tuple(string, int)]: Represents the network endpoint as an IP address 181 | and port number. 182 | """ 183 | 184 | try: 185 | # The structure of the sensor message for reading the data. 186 | sensor_message = egm_pb2.EgmSensor() 187 | 188 | # Sent by the sensor, MSGTYPE_DATA if sent from the robot controller. 189 | egm_header = sensor_message.header 190 | egm_header.mtype = egm_pb2.EgmHeader.MessageType.MSGTYPE_CORRECTION 191 | 192 | # Binding the planned data in the form of absolute robot joints to the sensor object. 193 | egm_planned = sensor_message.planned 194 | if enable_external_joint == True: 195 | egm_planned.joints.joints.extend(theta[0:-1]) 196 | egm_planned.externalJoints.joints.extend([theta[-1]]) 197 | else: 198 | egm_planned.joints.joints.extend(theta) 199 | 200 | # Serialize the data to be sent. 201 | data = sensor_message.SerializeToString() 202 | 203 | # Sending the created message to the robot, which includes the desired absolute 204 | # position of the robot's joints via UDP. 205 | self.__udp_client.sendto(data, end_point) 206 | 207 | except socket.error as e: 208 | print(f'[ERROR] Information: {e}') 209 | print('[ERROR] An error occurred while sending the socket.') 210 | 211 | def Set_Absolute_Joint_Position(self, theta: tp.List[float], enable_external_joint: bool) -> None: 212 | """ 213 | Description: 214 | Set the absolute position of the robot joints. 215 | 216 | Note: 217 | The function also reads data (cartesian, joints, etc.) from the robot. 218 | 219 | Args: 220 | (1) theta [Vector 1xn]: Desired absolute joint position in radians. 221 | Note: 222 | Where n is the number of joints. 223 | (2) enable_external_joint [bool]: Information about whether or not the robot has an external joint. 224 | """ 225 | 226 | try: 227 | # Obtain the data from the robot via UDP. 228 | # Note: 229 | # data: A byte-type array that contains the datagram data. 230 | # end_point: Represents the network endpoint as an IP address 231 | # and port number. 232 | (data, end_point) = self.__udp_client.recvfrom(CONST_BUFSIZE) 233 | 234 | if data is not None: 235 | # Parse the information from the collected data. 236 | self.__sensor_message_r.ParseFromString(data) 237 | 238 | # Express the actual machine time. 239 | self.__t = self.__sensor_message_r.header.tm 240 | 241 | # Express the actual position and orientation of the tool center point (TCP). 242 | # Position: p = x, y, z in millimeters. 243 | p = np.array([self.__sensor_message_r.feedBack.cartesian.pos.x, 244 | self.__sensor_message_r.feedBack.cartesian.pos.y, 245 | self.__sensor_message_r.feedBack.cartesian.pos.z], dtype=np.float64) 246 | # Orientation: q = w, x, y, z in [-] -> [-1.0, 1.0]. 247 | q = np.array([self.__sensor_message_r.feedBack.cartesian.orient.u0, 248 | self.__sensor_message_r.feedBack.cartesian.orient.u1, 249 | self.__sensor_message_r.feedBack.cartesian.orient.u2, 250 | self.__sensor_message_r.feedBack.cartesian.orient.u3], dtype=np.float64) 251 | 252 | # Express the obtained parameters as a homogeneous transformation matrix. 253 | self.__T_EE = HTM_Cls(None, np.float64).Rotation(q, 'QUATERNION').Translation(p) 254 | 255 | # Express the absolute positions of the robot's joints. 256 | if enable_external_joint == True: 257 | self.__theta = np.append(np.array(self.__sensor_message_r.feedBack.joints.joints, dtype=np.float64), 258 | np.array(self.__sensor_message_r.feedBack.externalJoints.joints, dtype=np.float64)) 259 | else: 260 | self.__theta = np.array(self.__sensor_message_r.feedBack.joints.joints, dtype=np.float64) 261 | 262 | # Send data to the robot. 263 | self.__Send_Sensor_Message(Mathematics.Radian_To_Degree(theta), enable_external_joint, end_point) 264 | 265 | except socket.error as e: 266 | print(f'[ERROR] Information: {e}') 267 | print('[ERROR] An error occurred while receiving the socket.') -------------------------------------------------------------------------------- /src/Lib/EGM/egm_pb2.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # Generated by the protocol buffer compiler. DO NOT EDIT! 3 | # source: my.proto 4 | 5 | from google.protobuf import descriptor as _descriptor 6 | from google.protobuf import message as _message 7 | from google.protobuf import reflection as _reflection 8 | from google.protobuf import symbol_database as _symbol_database 9 | # @@protoc_insertion_point(imports) 10 | 11 | _sym_db = _symbol_database.Default() 12 | 13 | DESCRIPTOR = _descriptor.FileDescriptor( 14 | name='my.proto', 15 | package='abb.egm', 16 | syntax='proto2', 17 | serialized_options=None, 18 | create_key=_descriptor._internal_create_key, 19 | serialized_pb=b'\n\x08my.proto\x12\x07\x61\x62\x62.egm\"\xeb\x01\n\tEgmHeader\x12\r\n\x05seqno\x18\x01 \x01(\r\x12\n\n\x02tm\x18\x02 \x01(\r\x12@\n\x05mtype\x18\x03 \x01(\x0e\x32\x1e.abb.egm.EgmHeader.MessageType:\x11MSGTYPE_UNDEFINED\"\x80\x01\n\x0bMessageType\x12\x15\n\x11MSGTYPE_UNDEFINED\x10\x00\x12\x13\n\x0fMSGTYPE_COMMAND\x10\x01\x12\x10\n\x0cMSGTYPE_DATA\x10\x02\x12\x16\n\x12MSGTYPE_CORRECTION\x10\x03\x12\x1b\n\x17MSGTYPE_PATH_CORRECTION\x10\x04\"/\n\x0c\x45gmCartesian\x12\t\n\x01x\x18\x01 \x02(\x01\x12\t\n\x01y\x18\x02 \x02(\x01\x12\t\n\x01z\x18\x03 \x02(\x01\"?\n\rEgmQuaternion\x12\n\n\x02u0\x18\x01 \x02(\x01\x12\n\n\x02u1\x18\x02 \x02(\x01\x12\n\n\x02u2\x18\x03 \x02(\x01\x12\n\n\x02u3\x18\x04 \x02(\x01\"+\n\x08\x45gmEuler\x12\t\n\x01x\x18\x01 \x02(\x01\x12\t\n\x01y\x18\x02 \x02(\x01\x12\t\n\x01z\x18\x03 \x02(\x01\"%\n\x08\x45gmClock\x12\x0b\n\x03sec\x18\x01 \x02(\x04\x12\x0c\n\x04usec\x18\x02 \x02(\x04\"w\n\x07\x45gmPose\x12\"\n\x03pos\x18\x01 \x01(\x0b\x32\x15.abb.egm.EgmCartesian\x12&\n\x06orient\x18\x02 \x01(\x0b\x32\x16.abb.egm.EgmQuaternion\x12 \n\x05\x65uler\x18\x03 \x01(\x0b\x32\x11.abb.egm.EgmEuler\"\"\n\x11\x45gmCartesianSpeed\x12\r\n\x05value\x18\x01 \x03(\x01\"\x1b\n\tEgmJoints\x12\x0e\n\x06joints\x18\x01 \x03(\x01\"#\n\x11\x45gmExternalJoints\x12\x0e\n\x06joints\x18\x01 \x03(\x01\"\xa2\x01\n\nEgmPlanned\x12\"\n\x06joints\x18\x01 \x01(\x0b\x32\x12.abb.egm.EgmJoints\x12#\n\tcartesian\x18\x02 \x01(\x0b\x32\x10.abb.egm.EgmPose\x12*\n\x0e\x65xternalJoints\x18\x03 \x01(\x0b\x32\x12.abb.egm.EgmJoints\x12\x1f\n\x04time\x18\x04 \x01(\x0b\x32\x11.abb.egm.EgmClock\"\x8d\x01\n\x0b\x45gmSpeedRef\x12\"\n\x06joints\x18\x01 \x01(\x0b\x32\x12.abb.egm.EgmJoints\x12.\n\ncartesians\x18\x02 \x01(\x0b\x32\x1a.abb.egm.EgmCartesianSpeed\x12*\n\x0e\x65xternalJoints\x18\x03 \x01(\x0b\x32\x12.abb.egm.EgmJoints\">\n\x0b\x45gmPathCorr\x12\"\n\x03pos\x18\x01 \x02(\x0b\x32\x15.abb.egm.EgmCartesian\x12\x0b\n\x03\x61ge\x18\x02 \x02(\r\"\xa3\x01\n\x0b\x45gmFeedBack\x12\"\n\x06joints\x18\x01 \x01(\x0b\x32\x12.abb.egm.EgmJoints\x12#\n\tcartesian\x18\x02 \x01(\x0b\x32\x10.abb.egm.EgmPose\x12*\n\x0e\x65xternalJoints\x18\x03 \x01(\x0b\x32\x12.abb.egm.EgmJoints\x12\x1f\n\x04time\x18\x04 \x01(\x0b\x32\x11.abb.egm.EgmClock\"\x8c\x01\n\rEgmMotorState\x12\x34\n\x05state\x18\x01 \x02(\x0e\x32%.abb.egm.EgmMotorState.MotorStateType\"E\n\x0eMotorStateType\x12\x14\n\x10MOTORS_UNDEFINED\x10\x00\x12\r\n\tMOTORS_ON\x10\x01\x12\x0e\n\nMOTORS_OFF\x10\x02\"\xa2\x01\n\x0b\x45gmMCIState\x12?\n\x05state\x18\x01 \x02(\x0e\x32!.abb.egm.EgmMCIState.MCIStateType:\rMCI_UNDEFINED\"R\n\x0cMCIStateType\x12\x11\n\rMCI_UNDEFINED\x10\x00\x12\r\n\tMCI_ERROR\x10\x01\x12\x0f\n\x0bMCI_STOPPED\x10\x02\x12\x0f\n\x0bMCI_RUNNING\x10\x03\"\xc3\x01\n\x15\x45gmRapidCtrlExecState\x12U\n\x05state\x18\x01 \x02(\x0e\x32\x35.abb.egm.EgmRapidCtrlExecState.RapidCtrlExecStateType:\x0fRAPID_UNDEFINED\"S\n\x16RapidCtrlExecStateType\x12\x13\n\x0fRAPID_UNDEFINED\x10\x00\x12\x11\n\rRAPID_STOPPED\x10\x01\x12\x11\n\rRAPID_RUNNING\x10\x02\"!\n\x0e\x45gmTestSignals\x12\x0f\n\x07signals\x18\x01 \x03(\x01\"!\n\x10\x45gmMeasuredForce\x12\r\n\x05\x66orce\x18\x01 \x03(\x01\"\x9c\x03\n\x08\x45gmRobot\x12\"\n\x06header\x18\x01 \x01(\x0b\x32\x12.abb.egm.EgmHeader\x12&\n\x08\x66\x65\x65\x64\x42\x61\x63k\x18\x02 \x01(\x0b\x32\x14.abb.egm.EgmFeedBack\x12$\n\x07planned\x18\x03 \x01(\x0b\x32\x13.abb.egm.EgmPlanned\x12*\n\nmotorState\x18\x04 \x01(\x0b\x32\x16.abb.egm.EgmMotorState\x12&\n\x08mciState\x18\x05 \x01(\x0b\x32\x14.abb.egm.EgmMCIState\x12\x19\n\x11mciConvergenceMet\x18\x06 \x01(\x08\x12,\n\x0btestSignals\x18\x07 \x01(\x0b\x32\x17.abb.egm.EgmTestSignals\x12\x36\n\x0erapidExecState\x18\x08 \x01(\x0b\x32\x1e.abb.egm.EgmRapidCtrlExecState\x12\x30\n\rmeasuredForce\x18\t \x01(\x0b\x32\x19.abb.egm.EgmMeasuredForce\x12\x17\n\x0futilizationRate\x18\n \x01(\x01\"}\n\tEgmSensor\x12\"\n\x06header\x18\x01 \x01(\x0b\x32\x12.abb.egm.EgmHeader\x12$\n\x07planned\x18\x02 \x01(\x0b\x32\x13.abb.egm.EgmPlanned\x12&\n\x08speedRef\x18\x03 \x01(\x0b\x32\x14.abb.egm.EgmSpeedRef\"_\n\x11\x45gmSensorPathCorr\x12\"\n\x06header\x18\x01 \x01(\x0b\x32\x12.abb.egm.EgmHeader\x12&\n\x08pathCorr\x18\x02 \x01(\x0b\x32\x14.abb.egm.EgmPathCorr' 20 | ) 21 | 22 | _EGMHEADER_MESSAGETYPE = _descriptor.EnumDescriptor( 23 | name='MessageType', 24 | full_name='abb.egm.EgmHeader.MessageType', 25 | filename=None, 26 | file=DESCRIPTOR, 27 | create_key=_descriptor._internal_create_key, 28 | values=[ 29 | _descriptor.EnumValueDescriptor( 30 | name='MSGTYPE_UNDEFINED', index=0, number=0, 31 | serialized_options=None, 32 | type=None, 33 | create_key=_descriptor._internal_create_key), 34 | _descriptor.EnumValueDescriptor( 35 | name='MSGTYPE_COMMAND', index=1, number=1, 36 | serialized_options=None, 37 | type=None, 38 | create_key=_descriptor._internal_create_key), 39 | _descriptor.EnumValueDescriptor( 40 | name='MSGTYPE_DATA', index=2, number=2, 41 | serialized_options=None, 42 | type=None, 43 | create_key=_descriptor._internal_create_key), 44 | _descriptor.EnumValueDescriptor( 45 | name='MSGTYPE_CORRECTION', index=3, number=3, 46 | serialized_options=None, 47 | type=None, 48 | create_key=_descriptor._internal_create_key), 49 | _descriptor.EnumValueDescriptor( 50 | name='MSGTYPE_PATH_CORRECTION', index=4, number=4, 51 | serialized_options=None, 52 | type=None, 53 | create_key=_descriptor._internal_create_key), 54 | ], 55 | containing_type=None, 56 | serialized_options=None, 57 | serialized_start=129, 58 | serialized_end=257, 59 | ) 60 | _sym_db.RegisterEnumDescriptor(_EGMHEADER_MESSAGETYPE) 61 | 62 | _EGMMOTORSTATE_MOTORSTATETYPE = _descriptor.EnumDescriptor( 63 | name='MotorStateType', 64 | full_name='abb.egm.EgmMotorState.MotorStateType', 65 | filename=None, 66 | file=DESCRIPTOR, 67 | create_key=_descriptor._internal_create_key, 68 | values=[ 69 | _descriptor.EnumValueDescriptor( 70 | name='MOTORS_UNDEFINED', index=0, number=0, 71 | serialized_options=None, 72 | type=None, 73 | create_key=_descriptor._internal_create_key), 74 | _descriptor.EnumValueDescriptor( 75 | name='MOTORS_ON', index=1, number=1, 76 | serialized_options=None, 77 | type=None, 78 | create_key=_descriptor._internal_create_key), 79 | _descriptor.EnumValueDescriptor( 80 | name='MOTORS_OFF', index=2, number=2, 81 | serialized_options=None, 82 | type=None, 83 | create_key=_descriptor._internal_create_key), 84 | ], 85 | containing_type=None, 86 | serialized_options=None, 87 | serialized_start=1291, 88 | serialized_end=1360, 89 | ) 90 | _sym_db.RegisterEnumDescriptor(_EGMMOTORSTATE_MOTORSTATETYPE) 91 | 92 | _EGMMCISTATE_MCISTATETYPE = _descriptor.EnumDescriptor( 93 | name='MCIStateType', 94 | full_name='abb.egm.EgmMCIState.MCIStateType', 95 | filename=None, 96 | file=DESCRIPTOR, 97 | create_key=_descriptor._internal_create_key, 98 | values=[ 99 | _descriptor.EnumValueDescriptor( 100 | name='MCI_UNDEFINED', index=0, number=0, 101 | serialized_options=None, 102 | type=None, 103 | create_key=_descriptor._internal_create_key), 104 | _descriptor.EnumValueDescriptor( 105 | name='MCI_ERROR', index=1, number=1, 106 | serialized_options=None, 107 | type=None, 108 | create_key=_descriptor._internal_create_key), 109 | _descriptor.EnumValueDescriptor( 110 | name='MCI_STOPPED', index=2, number=2, 111 | serialized_options=None, 112 | type=None, 113 | create_key=_descriptor._internal_create_key), 114 | _descriptor.EnumValueDescriptor( 115 | name='MCI_RUNNING', index=3, number=3, 116 | serialized_options=None, 117 | type=None, 118 | create_key=_descriptor._internal_create_key), 119 | ], 120 | containing_type=None, 121 | serialized_options=None, 122 | serialized_start=1443, 123 | serialized_end=1525, 124 | ) 125 | _sym_db.RegisterEnumDescriptor(_EGMMCISTATE_MCISTATETYPE) 126 | 127 | _EGMRAPIDCTRLEXECSTATE_RAPIDCTRLEXECSTATETYPE = _descriptor.EnumDescriptor( 128 | name='RapidCtrlExecStateType', 129 | full_name='abb.egm.EgmRapidCtrlExecState.RapidCtrlExecStateType', 130 | filename=None, 131 | file=DESCRIPTOR, 132 | create_key=_descriptor._internal_create_key, 133 | values=[ 134 | _descriptor.EnumValueDescriptor( 135 | name='RAPID_UNDEFINED', index=0, number=0, 136 | serialized_options=None, 137 | type=None, 138 | create_key=_descriptor._internal_create_key), 139 | _descriptor.EnumValueDescriptor( 140 | name='RAPID_STOPPED', index=1, number=1, 141 | serialized_options=None, 142 | type=None, 143 | create_key=_descriptor._internal_create_key), 144 | _descriptor.EnumValueDescriptor( 145 | name='RAPID_RUNNING', index=2, number=2, 146 | serialized_options=None, 147 | type=None, 148 | create_key=_descriptor._internal_create_key), 149 | ], 150 | containing_type=None, 151 | serialized_options=None, 152 | serialized_start=1640, 153 | serialized_end=1723, 154 | ) 155 | _sym_db.RegisterEnumDescriptor(_EGMRAPIDCTRLEXECSTATE_RAPIDCTRLEXECSTATETYPE) 156 | 157 | 158 | _EGMHEADER = _descriptor.Descriptor( 159 | name='EgmHeader', 160 | full_name='abb.egm.EgmHeader', 161 | filename=None, 162 | file=DESCRIPTOR, 163 | containing_type=None, 164 | create_key=_descriptor._internal_create_key, 165 | fields=[ 166 | _descriptor.FieldDescriptor( 167 | name='seqno', full_name='abb.egm.EgmHeader.seqno', index=0, 168 | number=1, type=13, cpp_type=3, label=1, 169 | has_default_value=False, default_value=0, 170 | message_type=None, enum_type=None, containing_type=None, 171 | is_extension=False, extension_scope=None, 172 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 173 | _descriptor.FieldDescriptor( 174 | name='tm', full_name='abb.egm.EgmHeader.tm', index=1, 175 | number=2, type=13, cpp_type=3, label=1, 176 | has_default_value=False, default_value=0, 177 | message_type=None, enum_type=None, containing_type=None, 178 | is_extension=False, extension_scope=None, 179 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 180 | _descriptor.FieldDescriptor( 181 | name='mtype', full_name='abb.egm.EgmHeader.mtype', index=2, 182 | number=3, type=14, cpp_type=8, label=1, 183 | has_default_value=True, default_value=0, 184 | message_type=None, enum_type=None, containing_type=None, 185 | is_extension=False, extension_scope=None, 186 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 187 | ], 188 | extensions=[ 189 | ], 190 | nested_types=[], 191 | enum_types=[ 192 | _EGMHEADER_MESSAGETYPE, 193 | ], 194 | serialized_options=None, 195 | is_extendable=False, 196 | syntax='proto2', 197 | extension_ranges=[], 198 | oneofs=[ 199 | ], 200 | serialized_start=22, 201 | serialized_end=257, 202 | ) 203 | 204 | 205 | _EGMCARTESIAN = _descriptor.Descriptor( 206 | name='EgmCartesian', 207 | full_name='abb.egm.EgmCartesian', 208 | filename=None, 209 | file=DESCRIPTOR, 210 | containing_type=None, 211 | create_key=_descriptor._internal_create_key, 212 | fields=[ 213 | _descriptor.FieldDescriptor( 214 | name='x', full_name='abb.egm.EgmCartesian.x', index=0, 215 | number=1, type=1, cpp_type=5, label=2, 216 | has_default_value=False, default_value=float(0), 217 | message_type=None, enum_type=None, containing_type=None, 218 | is_extension=False, extension_scope=None, 219 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 220 | _descriptor.FieldDescriptor( 221 | name='y', full_name='abb.egm.EgmCartesian.y', index=1, 222 | number=2, type=1, cpp_type=5, label=2, 223 | has_default_value=False, default_value=float(0), 224 | message_type=None, enum_type=None, containing_type=None, 225 | is_extension=False, extension_scope=None, 226 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 227 | _descriptor.FieldDescriptor( 228 | name='z', full_name='abb.egm.EgmCartesian.z', index=2, 229 | number=3, type=1, cpp_type=5, label=2, 230 | has_default_value=False, default_value=float(0), 231 | message_type=None, enum_type=None, containing_type=None, 232 | is_extension=False, extension_scope=None, 233 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 234 | ], 235 | extensions=[ 236 | ], 237 | nested_types=[], 238 | enum_types=[ 239 | ], 240 | serialized_options=None, 241 | is_extendable=False, 242 | syntax='proto2', 243 | extension_ranges=[], 244 | oneofs=[ 245 | ], 246 | serialized_start=259, 247 | serialized_end=306, 248 | ) 249 | 250 | 251 | _EGMQUATERNION = _descriptor.Descriptor( 252 | name='EgmQuaternion', 253 | full_name='abb.egm.EgmQuaternion', 254 | filename=None, 255 | file=DESCRIPTOR, 256 | containing_type=None, 257 | create_key=_descriptor._internal_create_key, 258 | fields=[ 259 | _descriptor.FieldDescriptor( 260 | name='u0', full_name='abb.egm.EgmQuaternion.u0', index=0, 261 | number=1, type=1, cpp_type=5, label=2, 262 | has_default_value=False, default_value=float(0), 263 | message_type=None, enum_type=None, containing_type=None, 264 | is_extension=False, extension_scope=None, 265 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 266 | _descriptor.FieldDescriptor( 267 | name='u1', full_name='abb.egm.EgmQuaternion.u1', index=1, 268 | number=2, type=1, cpp_type=5, label=2, 269 | has_default_value=False, default_value=float(0), 270 | message_type=None, enum_type=None, containing_type=None, 271 | is_extension=False, extension_scope=None, 272 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 273 | _descriptor.FieldDescriptor( 274 | name='u2', full_name='abb.egm.EgmQuaternion.u2', index=2, 275 | number=3, type=1, cpp_type=5, label=2, 276 | has_default_value=False, default_value=float(0), 277 | message_type=None, enum_type=None, containing_type=None, 278 | is_extension=False, extension_scope=None, 279 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 280 | _descriptor.FieldDescriptor( 281 | name='u3', full_name='abb.egm.EgmQuaternion.u3', index=3, 282 | number=4, type=1, cpp_type=5, label=2, 283 | has_default_value=False, default_value=float(0), 284 | message_type=None, enum_type=None, containing_type=None, 285 | is_extension=False, extension_scope=None, 286 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 287 | ], 288 | extensions=[ 289 | ], 290 | nested_types=[], 291 | enum_types=[ 292 | ], 293 | serialized_options=None, 294 | is_extendable=False, 295 | syntax='proto2', 296 | extension_ranges=[], 297 | oneofs=[ 298 | ], 299 | serialized_start=308, 300 | serialized_end=371, 301 | ) 302 | 303 | 304 | _EGMEULER = _descriptor.Descriptor( 305 | name='EgmEuler', 306 | full_name='abb.egm.EgmEuler', 307 | filename=None, 308 | file=DESCRIPTOR, 309 | containing_type=None, 310 | create_key=_descriptor._internal_create_key, 311 | fields=[ 312 | _descriptor.FieldDescriptor( 313 | name='x', full_name='abb.egm.EgmEuler.x', index=0, 314 | number=1, type=1, cpp_type=5, label=2, 315 | has_default_value=False, default_value=float(0), 316 | message_type=None, enum_type=None, containing_type=None, 317 | is_extension=False, extension_scope=None, 318 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 319 | _descriptor.FieldDescriptor( 320 | name='y', full_name='abb.egm.EgmEuler.y', index=1, 321 | number=2, type=1, cpp_type=5, label=2, 322 | has_default_value=False, default_value=float(0), 323 | message_type=None, enum_type=None, containing_type=None, 324 | is_extension=False, extension_scope=None, 325 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 326 | _descriptor.FieldDescriptor( 327 | name='z', full_name='abb.egm.EgmEuler.z', index=2, 328 | number=3, type=1, cpp_type=5, label=2, 329 | has_default_value=False, default_value=float(0), 330 | message_type=None, enum_type=None, containing_type=None, 331 | is_extension=False, extension_scope=None, 332 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 333 | ], 334 | extensions=[ 335 | ], 336 | nested_types=[], 337 | enum_types=[ 338 | ], 339 | serialized_options=None, 340 | is_extendable=False, 341 | syntax='proto2', 342 | extension_ranges=[], 343 | oneofs=[ 344 | ], 345 | serialized_start=373, 346 | serialized_end=416, 347 | ) 348 | 349 | 350 | _EGMCLOCK = _descriptor.Descriptor( 351 | name='EgmClock', 352 | full_name='abb.egm.EgmClock', 353 | filename=None, 354 | file=DESCRIPTOR, 355 | containing_type=None, 356 | create_key=_descriptor._internal_create_key, 357 | fields=[ 358 | _descriptor.FieldDescriptor( 359 | name='sec', full_name='abb.egm.EgmClock.sec', index=0, 360 | number=1, type=4, cpp_type=4, label=2, 361 | has_default_value=False, default_value=0, 362 | message_type=None, enum_type=None, containing_type=None, 363 | is_extension=False, extension_scope=None, 364 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 365 | _descriptor.FieldDescriptor( 366 | name='usec', full_name='abb.egm.EgmClock.usec', index=1, 367 | number=2, type=4, cpp_type=4, label=2, 368 | has_default_value=False, default_value=0, 369 | message_type=None, enum_type=None, containing_type=None, 370 | is_extension=False, extension_scope=None, 371 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 372 | ], 373 | extensions=[ 374 | ], 375 | nested_types=[], 376 | enum_types=[ 377 | ], 378 | serialized_options=None, 379 | is_extendable=False, 380 | syntax='proto2', 381 | extension_ranges=[], 382 | oneofs=[ 383 | ], 384 | serialized_start=418, 385 | serialized_end=455, 386 | ) 387 | 388 | 389 | _EGMPOSE = _descriptor.Descriptor( 390 | name='EgmPose', 391 | full_name='abb.egm.EgmPose', 392 | filename=None, 393 | file=DESCRIPTOR, 394 | containing_type=None, 395 | create_key=_descriptor._internal_create_key, 396 | fields=[ 397 | _descriptor.FieldDescriptor( 398 | name='pos', full_name='abb.egm.EgmPose.pos', index=0, 399 | number=1, type=11, cpp_type=10, label=1, 400 | has_default_value=False, default_value=None, 401 | message_type=None, enum_type=None, containing_type=None, 402 | is_extension=False, extension_scope=None, 403 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 404 | _descriptor.FieldDescriptor( 405 | name='orient', full_name='abb.egm.EgmPose.orient', index=1, 406 | number=2, type=11, cpp_type=10, label=1, 407 | has_default_value=False, default_value=None, 408 | message_type=None, enum_type=None, containing_type=None, 409 | is_extension=False, extension_scope=None, 410 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 411 | _descriptor.FieldDescriptor( 412 | name='euler', full_name='abb.egm.EgmPose.euler', index=2, 413 | number=3, type=11, cpp_type=10, label=1, 414 | has_default_value=False, default_value=None, 415 | message_type=None, enum_type=None, containing_type=None, 416 | is_extension=False, extension_scope=None, 417 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 418 | ], 419 | extensions=[ 420 | ], 421 | nested_types=[], 422 | enum_types=[ 423 | ], 424 | serialized_options=None, 425 | is_extendable=False, 426 | syntax='proto2', 427 | extension_ranges=[], 428 | oneofs=[ 429 | ], 430 | serialized_start=457, 431 | serialized_end=576, 432 | ) 433 | 434 | 435 | _EGMCARTESIANSPEED = _descriptor.Descriptor( 436 | name='EgmCartesianSpeed', 437 | full_name='abb.egm.EgmCartesianSpeed', 438 | filename=None, 439 | file=DESCRIPTOR, 440 | containing_type=None, 441 | create_key=_descriptor._internal_create_key, 442 | fields=[ 443 | _descriptor.FieldDescriptor( 444 | name='value', full_name='abb.egm.EgmCartesianSpeed.value', index=0, 445 | number=1, type=1, cpp_type=5, label=3, 446 | has_default_value=False, default_value=[], 447 | message_type=None, enum_type=None, containing_type=None, 448 | is_extension=False, extension_scope=None, 449 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 450 | ], 451 | extensions=[ 452 | ], 453 | nested_types=[], 454 | enum_types=[ 455 | ], 456 | serialized_options=None, 457 | is_extendable=False, 458 | syntax='proto2', 459 | extension_ranges=[], 460 | oneofs=[ 461 | ], 462 | serialized_start=578, 463 | serialized_end=612, 464 | ) 465 | 466 | 467 | _EGMJOINTS = _descriptor.Descriptor( 468 | name='EgmJoints', 469 | full_name='abb.egm.EgmJoints', 470 | filename=None, 471 | file=DESCRIPTOR, 472 | containing_type=None, 473 | create_key=_descriptor._internal_create_key, 474 | fields=[ 475 | _descriptor.FieldDescriptor( 476 | name='joints', full_name='abb.egm.EgmJoints.joints', index=0, 477 | number=1, type=1, cpp_type=5, label=3, 478 | has_default_value=False, default_value=[], 479 | message_type=None, enum_type=None, containing_type=None, 480 | is_extension=False, extension_scope=None, 481 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 482 | ], 483 | extensions=[ 484 | ], 485 | nested_types=[], 486 | enum_types=[ 487 | ], 488 | serialized_options=None, 489 | is_extendable=False, 490 | syntax='proto2', 491 | extension_ranges=[], 492 | oneofs=[ 493 | ], 494 | serialized_start=614, 495 | serialized_end=641, 496 | ) 497 | 498 | 499 | _EGMEXTERNALJOINTS = _descriptor.Descriptor( 500 | name='EgmExternalJoints', 501 | full_name='abb.egm.EgmExternalJoints', 502 | filename=None, 503 | file=DESCRIPTOR, 504 | containing_type=None, 505 | create_key=_descriptor._internal_create_key, 506 | fields=[ 507 | _descriptor.FieldDescriptor( 508 | name='joints', full_name='abb.egm.EgmExternalJoints.joints', index=0, 509 | number=1, type=1, cpp_type=5, label=3, 510 | has_default_value=False, default_value=[], 511 | message_type=None, enum_type=None, containing_type=None, 512 | is_extension=False, extension_scope=None, 513 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 514 | ], 515 | extensions=[ 516 | ], 517 | nested_types=[], 518 | enum_types=[ 519 | ], 520 | serialized_options=None, 521 | is_extendable=False, 522 | syntax='proto2', 523 | extension_ranges=[], 524 | oneofs=[ 525 | ], 526 | serialized_start=643, 527 | serialized_end=678, 528 | ) 529 | 530 | 531 | _EGMPLANNED = _descriptor.Descriptor( 532 | name='EgmPlanned', 533 | full_name='abb.egm.EgmPlanned', 534 | filename=None, 535 | file=DESCRIPTOR, 536 | containing_type=None, 537 | create_key=_descriptor._internal_create_key, 538 | fields=[ 539 | _descriptor.FieldDescriptor( 540 | name='joints', full_name='abb.egm.EgmPlanned.joints', index=0, 541 | number=1, type=11, cpp_type=10, label=1, 542 | has_default_value=False, default_value=None, 543 | message_type=None, enum_type=None, containing_type=None, 544 | is_extension=False, extension_scope=None, 545 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 546 | _descriptor.FieldDescriptor( 547 | name='cartesian', full_name='abb.egm.EgmPlanned.cartesian', index=1, 548 | number=2, type=11, cpp_type=10, label=1, 549 | has_default_value=False, default_value=None, 550 | message_type=None, enum_type=None, containing_type=None, 551 | is_extension=False, extension_scope=None, 552 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 553 | _descriptor.FieldDescriptor( 554 | name='externalJoints', full_name='abb.egm.EgmPlanned.externalJoints', index=2, 555 | number=3, type=11, cpp_type=10, label=1, 556 | has_default_value=False, default_value=None, 557 | message_type=None, enum_type=None, containing_type=None, 558 | is_extension=False, extension_scope=None, 559 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 560 | _descriptor.FieldDescriptor( 561 | name='time', full_name='abb.egm.EgmPlanned.time', index=3, 562 | number=4, type=11, cpp_type=10, label=1, 563 | has_default_value=False, default_value=None, 564 | message_type=None, enum_type=None, containing_type=None, 565 | is_extension=False, extension_scope=None, 566 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 567 | ], 568 | extensions=[ 569 | ], 570 | nested_types=[], 571 | enum_types=[ 572 | ], 573 | serialized_options=None, 574 | is_extendable=False, 575 | syntax='proto2', 576 | extension_ranges=[], 577 | oneofs=[ 578 | ], 579 | serialized_start=681, 580 | serialized_end=843, 581 | ) 582 | 583 | 584 | _EGMSPEEDREF = _descriptor.Descriptor( 585 | name='EgmSpeedRef', 586 | full_name='abb.egm.EgmSpeedRef', 587 | filename=None, 588 | file=DESCRIPTOR, 589 | containing_type=None, 590 | create_key=_descriptor._internal_create_key, 591 | fields=[ 592 | _descriptor.FieldDescriptor( 593 | name='joints', full_name='abb.egm.EgmSpeedRef.joints', index=0, 594 | number=1, type=11, cpp_type=10, label=1, 595 | has_default_value=False, default_value=None, 596 | message_type=None, enum_type=None, containing_type=None, 597 | is_extension=False, extension_scope=None, 598 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 599 | _descriptor.FieldDescriptor( 600 | name='cartesians', full_name='abb.egm.EgmSpeedRef.cartesians', index=1, 601 | number=2, type=11, cpp_type=10, label=1, 602 | has_default_value=False, default_value=None, 603 | message_type=None, enum_type=None, containing_type=None, 604 | is_extension=False, extension_scope=None, 605 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 606 | _descriptor.FieldDescriptor( 607 | name='externalJoints', full_name='abb.egm.EgmSpeedRef.externalJoints', index=2, 608 | number=3, type=11, cpp_type=10, label=1, 609 | has_default_value=False, default_value=None, 610 | message_type=None, enum_type=None, containing_type=None, 611 | is_extension=False, extension_scope=None, 612 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 613 | ], 614 | extensions=[ 615 | ], 616 | nested_types=[], 617 | enum_types=[ 618 | ], 619 | serialized_options=None, 620 | is_extendable=False, 621 | syntax='proto2', 622 | extension_ranges=[], 623 | oneofs=[ 624 | ], 625 | serialized_start=846, 626 | serialized_end=987, 627 | ) 628 | 629 | 630 | _EGMPATHCORR = _descriptor.Descriptor( 631 | name='EgmPathCorr', 632 | full_name='abb.egm.EgmPathCorr', 633 | filename=None, 634 | file=DESCRIPTOR, 635 | containing_type=None, 636 | create_key=_descriptor._internal_create_key, 637 | fields=[ 638 | _descriptor.FieldDescriptor( 639 | name='pos', full_name='abb.egm.EgmPathCorr.pos', index=0, 640 | number=1, type=11, cpp_type=10, label=2, 641 | has_default_value=False, default_value=None, 642 | message_type=None, enum_type=None, containing_type=None, 643 | is_extension=False, extension_scope=None, 644 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 645 | _descriptor.FieldDescriptor( 646 | name='age', full_name='abb.egm.EgmPathCorr.age', index=1, 647 | number=2, type=13, cpp_type=3, label=2, 648 | has_default_value=False, default_value=0, 649 | message_type=None, enum_type=None, containing_type=None, 650 | is_extension=False, extension_scope=None, 651 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 652 | ], 653 | extensions=[ 654 | ], 655 | nested_types=[], 656 | enum_types=[ 657 | ], 658 | serialized_options=None, 659 | is_extendable=False, 660 | syntax='proto2', 661 | extension_ranges=[], 662 | oneofs=[ 663 | ], 664 | serialized_start=989, 665 | serialized_end=1051, 666 | ) 667 | 668 | 669 | _EGMFEEDBACK = _descriptor.Descriptor( 670 | name='EgmFeedBack', 671 | full_name='abb.egm.EgmFeedBack', 672 | filename=None, 673 | file=DESCRIPTOR, 674 | containing_type=None, 675 | create_key=_descriptor._internal_create_key, 676 | fields=[ 677 | _descriptor.FieldDescriptor( 678 | name='joints', full_name='abb.egm.EgmFeedBack.joints', index=0, 679 | number=1, type=11, cpp_type=10, label=1, 680 | has_default_value=False, default_value=None, 681 | message_type=None, enum_type=None, containing_type=None, 682 | is_extension=False, extension_scope=None, 683 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 684 | _descriptor.FieldDescriptor( 685 | name='cartesian', full_name='abb.egm.EgmFeedBack.cartesian', index=1, 686 | number=2, type=11, cpp_type=10, label=1, 687 | has_default_value=False, default_value=None, 688 | message_type=None, enum_type=None, containing_type=None, 689 | is_extension=False, extension_scope=None, 690 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 691 | _descriptor.FieldDescriptor( 692 | name='externalJoints', full_name='abb.egm.EgmFeedBack.externalJoints', index=2, 693 | number=3, type=11, cpp_type=10, label=1, 694 | has_default_value=False, default_value=None, 695 | message_type=None, enum_type=None, containing_type=None, 696 | is_extension=False, extension_scope=None, 697 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 698 | _descriptor.FieldDescriptor( 699 | name='time', full_name='abb.egm.EgmFeedBack.time', index=3, 700 | number=4, type=11, cpp_type=10, label=1, 701 | has_default_value=False, default_value=None, 702 | message_type=None, enum_type=None, containing_type=None, 703 | is_extension=False, extension_scope=None, 704 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 705 | ], 706 | extensions=[ 707 | ], 708 | nested_types=[], 709 | enum_types=[ 710 | ], 711 | serialized_options=None, 712 | is_extendable=False, 713 | syntax='proto2', 714 | extension_ranges=[], 715 | oneofs=[ 716 | ], 717 | serialized_start=1054, 718 | serialized_end=1217, 719 | ) 720 | 721 | 722 | _EGMMOTORSTATE = _descriptor.Descriptor( 723 | name='EgmMotorState', 724 | full_name='abb.egm.EgmMotorState', 725 | filename=None, 726 | file=DESCRIPTOR, 727 | containing_type=None, 728 | create_key=_descriptor._internal_create_key, 729 | fields=[ 730 | _descriptor.FieldDescriptor( 731 | name='state', full_name='abb.egm.EgmMotorState.state', index=0, 732 | number=1, type=14, cpp_type=8, label=2, 733 | has_default_value=False, default_value=0, 734 | message_type=None, enum_type=None, containing_type=None, 735 | is_extension=False, extension_scope=None, 736 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 737 | ], 738 | extensions=[ 739 | ], 740 | nested_types=[], 741 | enum_types=[ 742 | _EGMMOTORSTATE_MOTORSTATETYPE, 743 | ], 744 | serialized_options=None, 745 | is_extendable=False, 746 | syntax='proto2', 747 | extension_ranges=[], 748 | oneofs=[ 749 | ], 750 | serialized_start=1220, 751 | serialized_end=1360, 752 | ) 753 | 754 | 755 | _EGMMCISTATE = _descriptor.Descriptor( 756 | name='EgmMCIState', 757 | full_name='abb.egm.EgmMCIState', 758 | filename=None, 759 | file=DESCRIPTOR, 760 | containing_type=None, 761 | create_key=_descriptor._internal_create_key, 762 | fields=[ 763 | _descriptor.FieldDescriptor( 764 | name='state', full_name='abb.egm.EgmMCIState.state', index=0, 765 | number=1, type=14, cpp_type=8, label=2, 766 | has_default_value=True, default_value=0, 767 | message_type=None, enum_type=None, containing_type=None, 768 | is_extension=False, extension_scope=None, 769 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 770 | ], 771 | extensions=[ 772 | ], 773 | nested_types=[], 774 | enum_types=[ 775 | _EGMMCISTATE_MCISTATETYPE, 776 | ], 777 | serialized_options=None, 778 | is_extendable=False, 779 | syntax='proto2', 780 | extension_ranges=[], 781 | oneofs=[ 782 | ], 783 | serialized_start=1363, 784 | serialized_end=1525, 785 | ) 786 | 787 | 788 | _EGMRAPIDCTRLEXECSTATE = _descriptor.Descriptor( 789 | name='EgmRapidCtrlExecState', 790 | full_name='abb.egm.EgmRapidCtrlExecState', 791 | filename=None, 792 | file=DESCRIPTOR, 793 | containing_type=None, 794 | create_key=_descriptor._internal_create_key, 795 | fields=[ 796 | _descriptor.FieldDescriptor( 797 | name='state', full_name='abb.egm.EgmRapidCtrlExecState.state', index=0, 798 | number=1, type=14, cpp_type=8, label=2, 799 | has_default_value=True, default_value=0, 800 | message_type=None, enum_type=None, containing_type=None, 801 | is_extension=False, extension_scope=None, 802 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 803 | ], 804 | extensions=[ 805 | ], 806 | nested_types=[], 807 | enum_types=[ 808 | _EGMRAPIDCTRLEXECSTATE_RAPIDCTRLEXECSTATETYPE, 809 | ], 810 | serialized_options=None, 811 | is_extendable=False, 812 | syntax='proto2', 813 | extension_ranges=[], 814 | oneofs=[ 815 | ], 816 | serialized_start=1528, 817 | serialized_end=1723, 818 | ) 819 | 820 | 821 | _EGMTESTSIGNALS = _descriptor.Descriptor( 822 | name='EgmTestSignals', 823 | full_name='abb.egm.EgmTestSignals', 824 | filename=None, 825 | file=DESCRIPTOR, 826 | containing_type=None, 827 | create_key=_descriptor._internal_create_key, 828 | fields=[ 829 | _descriptor.FieldDescriptor( 830 | name='signals', full_name='abb.egm.EgmTestSignals.signals', index=0, 831 | number=1, type=1, cpp_type=5, label=3, 832 | has_default_value=False, default_value=[], 833 | message_type=None, enum_type=None, containing_type=None, 834 | is_extension=False, extension_scope=None, 835 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 836 | ], 837 | extensions=[ 838 | ], 839 | nested_types=[], 840 | enum_types=[ 841 | ], 842 | serialized_options=None, 843 | is_extendable=False, 844 | syntax='proto2', 845 | extension_ranges=[], 846 | oneofs=[ 847 | ], 848 | serialized_start=1725, 849 | serialized_end=1758, 850 | ) 851 | 852 | 853 | _EGMMEASUREDFORCE = _descriptor.Descriptor( 854 | name='EgmMeasuredForce', 855 | full_name='abb.egm.EgmMeasuredForce', 856 | filename=None, 857 | file=DESCRIPTOR, 858 | containing_type=None, 859 | create_key=_descriptor._internal_create_key, 860 | fields=[ 861 | _descriptor.FieldDescriptor( 862 | name='force', full_name='abb.egm.EgmMeasuredForce.force', index=0, 863 | number=1, type=1, cpp_type=5, label=3, 864 | has_default_value=False, default_value=[], 865 | message_type=None, enum_type=None, containing_type=None, 866 | is_extension=False, extension_scope=None, 867 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 868 | ], 869 | extensions=[ 870 | ], 871 | nested_types=[], 872 | enum_types=[ 873 | ], 874 | serialized_options=None, 875 | is_extendable=False, 876 | syntax='proto2', 877 | extension_ranges=[], 878 | oneofs=[ 879 | ], 880 | serialized_start=1760, 881 | serialized_end=1793, 882 | ) 883 | 884 | 885 | _EGMROBOT = _descriptor.Descriptor( 886 | name='EgmRobot', 887 | full_name='abb.egm.EgmRobot', 888 | filename=None, 889 | file=DESCRIPTOR, 890 | containing_type=None, 891 | create_key=_descriptor._internal_create_key, 892 | fields=[ 893 | _descriptor.FieldDescriptor( 894 | name='header', full_name='abb.egm.EgmRobot.header', index=0, 895 | number=1, type=11, cpp_type=10, label=1, 896 | has_default_value=False, default_value=None, 897 | message_type=None, enum_type=None, containing_type=None, 898 | is_extension=False, extension_scope=None, 899 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 900 | _descriptor.FieldDescriptor( 901 | name='feedBack', full_name='abb.egm.EgmRobot.feedBack', index=1, 902 | number=2, type=11, cpp_type=10, label=1, 903 | has_default_value=False, default_value=None, 904 | message_type=None, enum_type=None, containing_type=None, 905 | is_extension=False, extension_scope=None, 906 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 907 | _descriptor.FieldDescriptor( 908 | name='planned', full_name='abb.egm.EgmRobot.planned', index=2, 909 | number=3, type=11, cpp_type=10, label=1, 910 | has_default_value=False, default_value=None, 911 | message_type=None, enum_type=None, containing_type=None, 912 | is_extension=False, extension_scope=None, 913 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 914 | _descriptor.FieldDescriptor( 915 | name='motorState', full_name='abb.egm.EgmRobot.motorState', index=3, 916 | number=4, type=11, cpp_type=10, label=1, 917 | has_default_value=False, default_value=None, 918 | message_type=None, enum_type=None, containing_type=None, 919 | is_extension=False, extension_scope=None, 920 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 921 | _descriptor.FieldDescriptor( 922 | name='mciState', full_name='abb.egm.EgmRobot.mciState', index=4, 923 | number=5, type=11, cpp_type=10, label=1, 924 | has_default_value=False, default_value=None, 925 | message_type=None, enum_type=None, containing_type=None, 926 | is_extension=False, extension_scope=None, 927 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 928 | _descriptor.FieldDescriptor( 929 | name='mciConvergenceMet', full_name='abb.egm.EgmRobot.mciConvergenceMet', index=5, 930 | number=6, type=8, cpp_type=7, label=1, 931 | has_default_value=False, default_value=False, 932 | message_type=None, enum_type=None, containing_type=None, 933 | is_extension=False, extension_scope=None, 934 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 935 | _descriptor.FieldDescriptor( 936 | name='testSignals', full_name='abb.egm.EgmRobot.testSignals', index=6, 937 | number=7, type=11, cpp_type=10, label=1, 938 | has_default_value=False, default_value=None, 939 | message_type=None, enum_type=None, containing_type=None, 940 | is_extension=False, extension_scope=None, 941 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 942 | _descriptor.FieldDescriptor( 943 | name='rapidExecState', full_name='abb.egm.EgmRobot.rapidExecState', index=7, 944 | number=8, type=11, cpp_type=10, label=1, 945 | has_default_value=False, default_value=None, 946 | message_type=None, enum_type=None, containing_type=None, 947 | is_extension=False, extension_scope=None, 948 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 949 | _descriptor.FieldDescriptor( 950 | name='measuredForce', full_name='abb.egm.EgmRobot.measuredForce', index=8, 951 | number=9, type=11, cpp_type=10, label=1, 952 | has_default_value=False, default_value=None, 953 | message_type=None, enum_type=None, containing_type=None, 954 | is_extension=False, extension_scope=None, 955 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 956 | _descriptor.FieldDescriptor( 957 | name='utilizationRate', full_name='abb.egm.EgmRobot.utilizationRate', index=9, 958 | number=10, type=1, cpp_type=5, label=1, 959 | has_default_value=False, default_value=float(0), 960 | message_type=None, enum_type=None, containing_type=None, 961 | is_extension=False, extension_scope=None, 962 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 963 | ], 964 | extensions=[ 965 | ], 966 | nested_types=[], 967 | enum_types=[ 968 | ], 969 | serialized_options=None, 970 | is_extendable=False, 971 | syntax='proto2', 972 | extension_ranges=[], 973 | oneofs=[ 974 | ], 975 | serialized_start=1796, 976 | serialized_end=2208, 977 | ) 978 | 979 | 980 | _EGMSENSOR = _descriptor.Descriptor( 981 | name='EgmSensor', 982 | full_name='abb.egm.EgmSensor', 983 | filename=None, 984 | file=DESCRIPTOR, 985 | containing_type=None, 986 | create_key=_descriptor._internal_create_key, 987 | fields=[ 988 | _descriptor.FieldDescriptor( 989 | name='header', full_name='abb.egm.EgmSensor.header', index=0, 990 | number=1, type=11, cpp_type=10, label=1, 991 | has_default_value=False, default_value=None, 992 | message_type=None, enum_type=None, containing_type=None, 993 | is_extension=False, extension_scope=None, 994 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 995 | _descriptor.FieldDescriptor( 996 | name='planned', full_name='abb.egm.EgmSensor.planned', index=1, 997 | number=2, type=11, cpp_type=10, label=1, 998 | has_default_value=False, default_value=None, 999 | message_type=None, enum_type=None, containing_type=None, 1000 | is_extension=False, extension_scope=None, 1001 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 1002 | _descriptor.FieldDescriptor( 1003 | name='speedRef', full_name='abb.egm.EgmSensor.speedRef', index=2, 1004 | number=3, type=11, cpp_type=10, label=1, 1005 | has_default_value=False, default_value=None, 1006 | message_type=None, enum_type=None, containing_type=None, 1007 | is_extension=False, extension_scope=None, 1008 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 1009 | ], 1010 | extensions=[ 1011 | ], 1012 | nested_types=[], 1013 | enum_types=[ 1014 | ], 1015 | serialized_options=None, 1016 | is_extendable=False, 1017 | syntax='proto2', 1018 | extension_ranges=[], 1019 | oneofs=[ 1020 | ], 1021 | serialized_start=2210, 1022 | serialized_end=2335, 1023 | ) 1024 | 1025 | 1026 | _EGMSENSORPATHCORR = _descriptor.Descriptor( 1027 | name='EgmSensorPathCorr', 1028 | full_name='abb.egm.EgmSensorPathCorr', 1029 | filename=None, 1030 | file=DESCRIPTOR, 1031 | containing_type=None, 1032 | create_key=_descriptor._internal_create_key, 1033 | fields=[ 1034 | _descriptor.FieldDescriptor( 1035 | name='header', full_name='abb.egm.EgmSensorPathCorr.header', index=0, 1036 | number=1, type=11, cpp_type=10, label=1, 1037 | has_default_value=False, default_value=None, 1038 | message_type=None, enum_type=None, containing_type=None, 1039 | is_extension=False, extension_scope=None, 1040 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 1041 | _descriptor.FieldDescriptor( 1042 | name='pathCorr', full_name='abb.egm.EgmSensorPathCorr.pathCorr', index=1, 1043 | number=2, type=11, cpp_type=10, label=1, 1044 | has_default_value=False, default_value=None, 1045 | message_type=None, enum_type=None, containing_type=None, 1046 | is_extension=False, extension_scope=None, 1047 | serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), 1048 | ], 1049 | extensions=[ 1050 | ], 1051 | nested_types=[], 1052 | enum_types=[ 1053 | ], 1054 | serialized_options=None, 1055 | is_extendable=False, 1056 | syntax='proto2', 1057 | extension_ranges=[], 1058 | oneofs=[ 1059 | ], 1060 | serialized_start=2337, 1061 | serialized_end=2432, 1062 | ) 1063 | 1064 | _EGMHEADER.fields_by_name['mtype'].enum_type = _EGMHEADER_MESSAGETYPE 1065 | _EGMHEADER_MESSAGETYPE.containing_type = _EGMHEADER 1066 | _EGMPOSE.fields_by_name['pos'].message_type = _EGMCARTESIAN 1067 | _EGMPOSE.fields_by_name['orient'].message_type = _EGMQUATERNION 1068 | _EGMPOSE.fields_by_name['euler'].message_type = _EGMEULER 1069 | _EGMPLANNED.fields_by_name['joints'].message_type = _EGMJOINTS 1070 | _EGMPLANNED.fields_by_name['cartesian'].message_type = _EGMPOSE 1071 | _EGMPLANNED.fields_by_name['externalJoints'].message_type = _EGMJOINTS 1072 | _EGMPLANNED.fields_by_name['time'].message_type = _EGMCLOCK 1073 | _EGMSPEEDREF.fields_by_name['joints'].message_type = _EGMJOINTS 1074 | _EGMSPEEDREF.fields_by_name['cartesians'].message_type = _EGMCARTESIANSPEED 1075 | _EGMSPEEDREF.fields_by_name['externalJoints'].message_type = _EGMJOINTS 1076 | _EGMPATHCORR.fields_by_name['pos'].message_type = _EGMCARTESIAN 1077 | _EGMFEEDBACK.fields_by_name['joints'].message_type = _EGMJOINTS 1078 | _EGMFEEDBACK.fields_by_name['cartesian'].message_type = _EGMPOSE 1079 | _EGMFEEDBACK.fields_by_name['externalJoints'].message_type = _EGMJOINTS 1080 | _EGMFEEDBACK.fields_by_name['time'].message_type = _EGMCLOCK 1081 | _EGMMOTORSTATE.fields_by_name['state'].enum_type = _EGMMOTORSTATE_MOTORSTATETYPE 1082 | _EGMMOTORSTATE_MOTORSTATETYPE.containing_type = _EGMMOTORSTATE 1083 | _EGMMCISTATE.fields_by_name['state'].enum_type = _EGMMCISTATE_MCISTATETYPE 1084 | _EGMMCISTATE_MCISTATETYPE.containing_type = _EGMMCISTATE 1085 | _EGMRAPIDCTRLEXECSTATE.fields_by_name['state'].enum_type = _EGMRAPIDCTRLEXECSTATE_RAPIDCTRLEXECSTATETYPE 1086 | _EGMRAPIDCTRLEXECSTATE_RAPIDCTRLEXECSTATETYPE.containing_type = _EGMRAPIDCTRLEXECSTATE 1087 | _EGMROBOT.fields_by_name['header'].message_type = _EGMHEADER 1088 | _EGMROBOT.fields_by_name['feedBack'].message_type = _EGMFEEDBACK 1089 | _EGMROBOT.fields_by_name['planned'].message_type = _EGMPLANNED 1090 | _EGMROBOT.fields_by_name['motorState'].message_type = _EGMMOTORSTATE 1091 | _EGMROBOT.fields_by_name['mciState'].message_type = _EGMMCISTATE 1092 | _EGMROBOT.fields_by_name['testSignals'].message_type = _EGMTESTSIGNALS 1093 | _EGMROBOT.fields_by_name['rapidExecState'].message_type = _EGMRAPIDCTRLEXECSTATE 1094 | _EGMROBOT.fields_by_name['measuredForce'].message_type = _EGMMEASUREDFORCE 1095 | _EGMSENSOR.fields_by_name['header'].message_type = _EGMHEADER 1096 | _EGMSENSOR.fields_by_name['planned'].message_type = _EGMPLANNED 1097 | _EGMSENSOR.fields_by_name['speedRef'].message_type = _EGMSPEEDREF 1098 | _EGMSENSORPATHCORR.fields_by_name['header'].message_type = _EGMHEADER 1099 | _EGMSENSORPATHCORR.fields_by_name['pathCorr'].message_type = _EGMPATHCORR 1100 | DESCRIPTOR.message_types_by_name['EgmHeader'] = _EGMHEADER 1101 | DESCRIPTOR.message_types_by_name['EgmCartesian'] = _EGMCARTESIAN 1102 | DESCRIPTOR.message_types_by_name['EgmQuaternion'] = _EGMQUATERNION 1103 | DESCRIPTOR.message_types_by_name['EgmEuler'] = _EGMEULER 1104 | DESCRIPTOR.message_types_by_name['EgmClock'] = _EGMCLOCK 1105 | DESCRIPTOR.message_types_by_name['EgmPose'] = _EGMPOSE 1106 | DESCRIPTOR.message_types_by_name['EgmCartesianSpeed'] = _EGMCARTESIANSPEED 1107 | DESCRIPTOR.message_types_by_name['EgmJoints'] = _EGMJOINTS 1108 | DESCRIPTOR.message_types_by_name['EgmExternalJoints'] = _EGMEXTERNALJOINTS 1109 | DESCRIPTOR.message_types_by_name['EgmPlanned'] = _EGMPLANNED 1110 | DESCRIPTOR.message_types_by_name['EgmSpeedRef'] = _EGMSPEEDREF 1111 | DESCRIPTOR.message_types_by_name['EgmPathCorr'] = _EGMPATHCORR 1112 | DESCRIPTOR.message_types_by_name['EgmFeedBack'] = _EGMFEEDBACK 1113 | DESCRIPTOR.message_types_by_name['EgmMotorState'] = _EGMMOTORSTATE 1114 | DESCRIPTOR.message_types_by_name['EgmMCIState'] = _EGMMCISTATE 1115 | DESCRIPTOR.message_types_by_name['EgmRapidCtrlExecState'] = _EGMRAPIDCTRLEXECSTATE 1116 | DESCRIPTOR.message_types_by_name['EgmTestSignals'] = _EGMTESTSIGNALS 1117 | DESCRIPTOR.message_types_by_name['EgmMeasuredForce'] = _EGMMEASUREDFORCE 1118 | DESCRIPTOR.message_types_by_name['EgmRobot'] = _EGMROBOT 1119 | DESCRIPTOR.message_types_by_name['EgmSensor'] = _EGMSENSOR 1120 | DESCRIPTOR.message_types_by_name['EgmSensorPathCorr'] = _EGMSENSORPATHCORR 1121 | _sym_db.RegisterFileDescriptor(DESCRIPTOR) 1122 | 1123 | EgmHeader = _reflection.GeneratedProtocolMessageType('EgmHeader', (_message.Message,), { 1124 | 'DESCRIPTOR' : _EGMHEADER, 1125 | '__module__' : 'my_pb2' 1126 | # @@protoc_insertion_point(class_scope:abb.egm.EgmHeader) 1127 | }) 1128 | _sym_db.RegisterMessage(EgmHeader) 1129 | 1130 | EgmCartesian = _reflection.GeneratedProtocolMessageType('EgmCartesian', (_message.Message,), { 1131 | 'DESCRIPTOR' : _EGMCARTESIAN, 1132 | '__module__' : 'my_pb2' 1133 | # @@protoc_insertion_point(class_scope:abb.egm.EgmCartesian) 1134 | }) 1135 | _sym_db.RegisterMessage(EgmCartesian) 1136 | 1137 | EgmQuaternion = _reflection.GeneratedProtocolMessageType('EgmQuaternion', (_message.Message,), { 1138 | 'DESCRIPTOR' : _EGMQUATERNION, 1139 | '__module__' : 'my_pb2' 1140 | # @@protoc_insertion_point(class_scope:abb.egm.EgmQuaternion) 1141 | }) 1142 | _sym_db.RegisterMessage(EgmQuaternion) 1143 | 1144 | EgmEuler = _reflection.GeneratedProtocolMessageType('EgmEuler', (_message.Message,), { 1145 | 'DESCRIPTOR' : _EGMEULER, 1146 | '__module__' : 'my_pb2' 1147 | # @@protoc_insertion_point(class_scope:abb.egm.EgmEuler) 1148 | }) 1149 | _sym_db.RegisterMessage(EgmEuler) 1150 | 1151 | EgmClock = _reflection.GeneratedProtocolMessageType('EgmClock', (_message.Message,), { 1152 | 'DESCRIPTOR' : _EGMCLOCK, 1153 | '__module__' : 'my_pb2' 1154 | # @@protoc_insertion_point(class_scope:abb.egm.EgmClock) 1155 | }) 1156 | _sym_db.RegisterMessage(EgmClock) 1157 | 1158 | EgmPose = _reflection.GeneratedProtocolMessageType('EgmPose', (_message.Message,), { 1159 | 'DESCRIPTOR' : _EGMPOSE, 1160 | '__module__' : 'my_pb2' 1161 | # @@protoc_insertion_point(class_scope:abb.egm.EgmPose) 1162 | }) 1163 | _sym_db.RegisterMessage(EgmPose) 1164 | 1165 | EgmCartesianSpeed = _reflection.GeneratedProtocolMessageType('EgmCartesianSpeed', (_message.Message,), { 1166 | 'DESCRIPTOR' : _EGMCARTESIANSPEED, 1167 | '__module__' : 'my_pb2' 1168 | # @@protoc_insertion_point(class_scope:abb.egm.EgmCartesianSpeed) 1169 | }) 1170 | _sym_db.RegisterMessage(EgmCartesianSpeed) 1171 | 1172 | EgmJoints = _reflection.GeneratedProtocolMessageType('EgmJoints', (_message.Message,), { 1173 | 'DESCRIPTOR' : _EGMJOINTS, 1174 | '__module__' : 'my_pb2' 1175 | # @@protoc_insertion_point(class_scope:abb.egm.EgmJoints) 1176 | }) 1177 | _sym_db.RegisterMessage(EgmJoints) 1178 | 1179 | EgmExternalJoints = _reflection.GeneratedProtocolMessageType('EgmExternalJoints', (_message.Message,), { 1180 | 'DESCRIPTOR' : _EGMEXTERNALJOINTS, 1181 | '__module__' : 'my_pb2' 1182 | # @@protoc_insertion_point(class_scope:abb.egm.EgmExternalJoints) 1183 | }) 1184 | _sym_db.RegisterMessage(EgmExternalJoints) 1185 | 1186 | EgmPlanned = _reflection.GeneratedProtocolMessageType('EgmPlanned', (_message.Message,), { 1187 | 'DESCRIPTOR' : _EGMPLANNED, 1188 | '__module__' : 'my_pb2' 1189 | # @@protoc_insertion_point(class_scope:abb.egm.EgmPlanned) 1190 | }) 1191 | _sym_db.RegisterMessage(EgmPlanned) 1192 | 1193 | EgmSpeedRef = _reflection.GeneratedProtocolMessageType('EgmSpeedRef', (_message.Message,), { 1194 | 'DESCRIPTOR' : _EGMSPEEDREF, 1195 | '__module__' : 'my_pb2' 1196 | # @@protoc_insertion_point(class_scope:abb.egm.EgmSpeedRef) 1197 | }) 1198 | _sym_db.RegisterMessage(EgmSpeedRef) 1199 | 1200 | EgmPathCorr = _reflection.GeneratedProtocolMessageType('EgmPathCorr', (_message.Message,), { 1201 | 'DESCRIPTOR' : _EGMPATHCORR, 1202 | '__module__' : 'my_pb2' 1203 | # @@protoc_insertion_point(class_scope:abb.egm.EgmPathCorr) 1204 | }) 1205 | _sym_db.RegisterMessage(EgmPathCorr) 1206 | 1207 | EgmFeedBack = _reflection.GeneratedProtocolMessageType('EgmFeedBack', (_message.Message,), { 1208 | 'DESCRIPTOR' : _EGMFEEDBACK, 1209 | '__module__' : 'my_pb2' 1210 | # @@protoc_insertion_point(class_scope:abb.egm.EgmFeedBack) 1211 | }) 1212 | _sym_db.RegisterMessage(EgmFeedBack) 1213 | 1214 | EgmMotorState = _reflection.GeneratedProtocolMessageType('EgmMotorState', (_message.Message,), { 1215 | 'DESCRIPTOR' : _EGMMOTORSTATE, 1216 | '__module__' : 'my_pb2' 1217 | # @@protoc_insertion_point(class_scope:abb.egm.EgmMotorState) 1218 | }) 1219 | _sym_db.RegisterMessage(EgmMotorState) 1220 | 1221 | EgmMCIState = _reflection.GeneratedProtocolMessageType('EgmMCIState', (_message.Message,), { 1222 | 'DESCRIPTOR' : _EGMMCISTATE, 1223 | '__module__' : 'my_pb2' 1224 | # @@protoc_insertion_point(class_scope:abb.egm.EgmMCIState) 1225 | }) 1226 | _sym_db.RegisterMessage(EgmMCIState) 1227 | 1228 | EgmRapidCtrlExecState = _reflection.GeneratedProtocolMessageType('EgmRapidCtrlExecState', (_message.Message,), { 1229 | 'DESCRIPTOR' : _EGMRAPIDCTRLEXECSTATE, 1230 | '__module__' : 'my_pb2' 1231 | # @@protoc_insertion_point(class_scope:abb.egm.EgmRapidCtrlExecState) 1232 | }) 1233 | _sym_db.RegisterMessage(EgmRapidCtrlExecState) 1234 | 1235 | EgmTestSignals = _reflection.GeneratedProtocolMessageType('EgmTestSignals', (_message.Message,), { 1236 | 'DESCRIPTOR' : _EGMTESTSIGNALS, 1237 | '__module__' : 'my_pb2' 1238 | # @@protoc_insertion_point(class_scope:abb.egm.EgmTestSignals) 1239 | }) 1240 | _sym_db.RegisterMessage(EgmTestSignals) 1241 | 1242 | EgmMeasuredForce = _reflection.GeneratedProtocolMessageType('EgmMeasuredForce', (_message.Message,), { 1243 | 'DESCRIPTOR' : _EGMMEASUREDFORCE, 1244 | '__module__' : 'my_pb2' 1245 | # @@protoc_insertion_point(class_scope:abb.egm.EgmMeasuredForce) 1246 | }) 1247 | _sym_db.RegisterMessage(EgmMeasuredForce) 1248 | 1249 | EgmRobot = _reflection.GeneratedProtocolMessageType('EgmRobot', (_message.Message,), { 1250 | 'DESCRIPTOR' : _EGMROBOT, 1251 | '__module__' : 'my_pb2' 1252 | # @@protoc_insertion_point(class_scope:abb.egm.EgmRobot) 1253 | }) 1254 | _sym_db.RegisterMessage(EgmRobot) 1255 | 1256 | EgmSensor = _reflection.GeneratedProtocolMessageType('EgmSensor', (_message.Message,), { 1257 | 'DESCRIPTOR' : _EGMSENSOR, 1258 | '__module__' : 'my_pb2' 1259 | # @@protoc_insertion_point(class_scope:abb.egm.EgmSensor) 1260 | }) 1261 | _sym_db.RegisterMessage(EgmSensor) 1262 | 1263 | EgmSensorPathCorr = _reflection.GeneratedProtocolMessageType('EgmSensorPathCorr', (_message.Message,), { 1264 | 'DESCRIPTOR' : _EGMSENSORPATHCORR, 1265 | '__module__' : 'my_pb2' 1266 | # @@protoc_insertion_point(class_scope:abb.egm.EgmSensorPathCorr) 1267 | }) 1268 | _sym_db.RegisterMessage(EgmSensorPathCorr) 1269 | 1270 | 1271 | # @@protoc_insertion_point(module_scope) 1272 | -------------------------------------------------------------------------------- /src/Lib/Trajectory/Core.py: -------------------------------------------------------------------------------- 1 | """ 2 | ## =========================================================================== ## 3 | MIT License 4 | Copyright (c) 2023 Roman Parak 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | The above copyright notice and this permission notice shall be included in all 12 | copies or substantial portions of the Software. 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. 20 | ## =========================================================================== ## 21 | Author : Roman Parak 22 | Email : Roman.Parak@outlook.com 23 | Github : https://github.com/rparak 24 | File Name: Core.py 25 | ## =========================================================================== ## 26 | """ 27 | 28 | # Numpy (Array computing) [pip3 install numpy] 29 | import numpy as np 30 | # Typing (Support for type hints) 31 | import typing as tp 32 | # Custom Lib.: 33 | # ../Lib/Trajectory/Utilities 34 | import Lib.Trajectory.Utilities 35 | # ../Lib/Transformation/Utilities/Mathematics 36 | import Lib.Transformation.Utilities.Mathematics as Mathematics 37 | 38 | """ 39 | Description: 40 | Some useful references about the field of trajectory generation: 41 | 1. Modern Robotics: Mechanics, Planning, and Control, Kevin M. Lynch and Frank C. Park 42 | 2. Trajectory Planning for Automatic Machines and Robots by Luigi Biagiotti, Claudio Melchiorri 43 | 3. Introduction To Robotics, Mechanics and Control, John J. Craig 44 | """ 45 | 46 | class Multi_Segment_Cls(object): 47 | """ 48 | Description: 49 | The specific class for generating the multi-segment trajectory using the two different methods. 50 | 51 | Initialization of the Class: 52 | Args: 53 | (1) method [string]: The method of the multi-segment trajectory generation. 54 | Note: 55 | method = 'Trapezoidal', 'Polynomial' 56 | (2) delta_time [float]: The difference (spacing) between the time values. 57 | 58 | Example: 59 | Initialization: 60 | # Assignment of the variables. 61 | method = 'Trepezoidal' 62 | delta_time = 0.01 63 | 64 | # Initialization of the class. 65 | Cls = Multi_Segment_Cls(method, delta_time) 66 | 67 | Features: 68 | # Properties of the class. 69 | Cls.t 70 | ... 71 | Cls.N 72 | 73 | # Functions of the class. 74 | Cls.Generate() 75 | """ 76 | 77 | def __init__(self, method: str, delta_time: float) -> None: 78 | try: 79 | assert method in ['Trapezoidal', 'Polynomial'] 80 | 81 | # The method of the multi-segment trajectory generation. 82 | self.__method = method 83 | 84 | # The difference (spacing) between the time values. 85 | self.__delta_time = delta_time 86 | 87 | except AssertionError as error: 88 | print(f'[ERROR] Information: {error}') 89 | print(f'[ERROR] Incorrect method name of the class input parameter. The method must be named as "Trapezoidal" or "Polynomial", not as "{method}".') 90 | 91 | @property 92 | def t(self) -> tp.List[float]: 93 | """ 94 | Description: 95 | Get evenly spaced time values at the following interval, see below: 96 | t_0 <= t <= t_f, 97 | 98 | where t_0 is 0.0 and t_f is is the sum of the duration between 99 | trajectory segments. 100 | 101 | Returns: 102 | (1) parameter [Vector 1xn]: Time values. 103 | Note: 104 | Where n is the number of time values. 105 | """ 106 | 107 | return self.__t 108 | 109 | @property 110 | def N(self) -> int: 111 | """ 112 | Description: 113 | Get the number of time points of the trajectory. 114 | 115 | Returns: 116 | (1) parameter [int]: Number of time points. 117 | """ 118 | 119 | return self.__t.size 120 | 121 | @property 122 | def delta_time(self) -> float: 123 | """ 124 | Description: 125 | Get the difference (spacing) between the time values. 126 | 127 | Returns: 128 | (1) parameter [float]: The difference (spacing) between the time values. 129 | """ 130 | 131 | return self.__delta_time 132 | 133 | @property 134 | def Method(self) -> str: 135 | """ 136 | Description: 137 | Get the method of the multi-segment trajectory generation. 138 | 139 | Returns: 140 | (1) parameter [float]: The method of the multi-segment trajectory generation. 141 | """ 142 | 143 | return self.__method 144 | 145 | def __Get_Arc_Length(self, s_dot: tp.List[float]) -> float: 146 | """ 147 | Description: 148 | Obtain the arc length L(t) of the position part of the trajectory. 149 | 150 | The arc length L(t) is defined by: 151 | L(t) = \int_{0}^{t} ||s'(t)||_{2} dt. 152 | 153 | Args: 154 | (1) s_dot [Vector 1xN]: Velocity part of the trapezoidal trajectory. 155 | Note: 156 | Where N is the number of time points of the trajectory. 157 | 158 | Returns: 159 | (1) parameter [float]: The arc length L(t) of the position part of the trajectory. 160 | """ 161 | 162 | L = 0.0 163 | for _, s_dot_i in enumerate(s_dot): 164 | L += Mathematics.Euclidean_Norm(s_dot_i) 165 | 166 | return L * self.__delta_time 167 | 168 | def __Generate_Trapezoidal(self, P: tp.List[tp.List[float]], t_blend: tp.List[float], T: tp.List[float], 169 | v: tp.List[float]) -> tp.Tuple[tp.List[float], 170 | tp.List[float], 171 | tp.List[float]]: 172 | """ 173 | Description: 174 | A function to generate position, velocity, and acceleration of a multi-segment trajectory using 175 | the trapezoidal trajectories method. 176 | 177 | Args: 178 | (1) P [Vector 1xn]: Input control points (waypoints) to be used for trajectory generation. 179 | (2) t_blend [Vector 1xn]: Duration of the blend phase. 180 | (3) T [Vector 1xn]: The velocity of each trajectory segment. 181 | (4) v [Vector 1x(n-1)]: The time of each trajectory segment. 182 | 183 | Returns: 184 | (1 - 3) parameter [Vector 1xN]: Position, velocity and acceleration trapezoidal trajectory. 185 | Note: 186 | Where N is the number of time points of the trajectory. 187 | 188 | Note: 189 | The variable n equals the number of points. 190 | """ 191 | 192 | # Initialization of the class to generate trajectory using using a trapezoidal 193 | # profile. 194 | T_Cls = Lib.Trajectory.Utilities.Trapezoidal_Profile_Cls(self.__delta_time) 195 | 196 | # Phase 1: The trajectory starts. 197 | (s_tmp, s_dot_tmp, s_ddot_tmp) = T_Cls.Generate(P[0], P[0] + v[0] * t_blend[0], 0.0, v[0], T[0] - t_blend[0], T[0] + t_blend[0]) 198 | self.__t = T_Cls.t 199 | s = s_tmp; s_dot = s_dot_tmp; s_ddot = s_ddot_tmp 200 | 201 | # Phase 2: The trajectory alternates between linear and blend phases. 202 | for _, (P_ii, v_i, v_ii, T_i, T_ii, t_blend_i, t_blend_ii) in enumerate(zip(P[1::], v, v[1::], T, T[1::], t_blend, t_blend[1::])): 203 | # Linear phase. 204 | (s_tmp, s_dot_tmp, s_ddot_tmp) = T_Cls.Generate(s[-1], s[-1] + v_i*((T_ii - t_blend_ii) - (T_i + t_blend_i)), v_i, v_i, T_i + t_blend_i, T_ii - t_blend_ii) 205 | self.__t = np.concatenate((self.__t, T_Cls.t), dtype=np.float64) 206 | s = np.concatenate((s, s_tmp), dtype=np.float64); s_dot = np.concatenate((s_dot, s_dot_tmp), dtype=np.float64); s_ddot = np.concatenate((s_ddot, s_ddot_tmp), dtype=np.float64) 207 | 208 | # Trapezoidal blends phase. 209 | (s_tmp, s_dot_tmp, s_ddot_tmp) = T_Cls.Generate(s[-1], P_ii + v_ii * t_blend_ii, v_i, v_ii, T_ii - t_blend_ii, T_ii + t_blend_ii) 210 | self.__t = np.concatenate((self.__t, T_Cls.t), dtype=np.float64) 211 | s = np.concatenate((s, s_tmp), dtype=np.float64); s_dot = np.concatenate((s_dot, s_dot_tmp), dtype=np.float64); s_ddot = np.concatenate((s_ddot, s_ddot_tmp), dtype=np.float64) 212 | 213 | # Phase 3: The trajectory ends. 214 | # Linear phase. 215 | (s_tmp, s_dot_tmp, s_ddot_tmp) = T_Cls.Generate(s[-1], s[-1] + v[-1]*((T[-1] - t_blend[-1]) - (T[-2] + t_blend[-2])), v[-1], v[-1], T[-2] + t_blend[-2], T[-1] - t_blend[-1]) 216 | self.__t = np.concatenate((self.__t, T_Cls.t), dtype=np.float64) 217 | s = np.concatenate((s, s_tmp), dtype=np.float64); s_dot = np.concatenate((s_dot, s_dot_tmp), dtype=np.float64); s_ddot = np.concatenate((s_ddot, s_ddot_tmp), dtype=np.float64) 218 | # Trapezoidal blends phase. 219 | (s_tmp, s_dot_tmp, s_ddot_tmp) = T_Cls.Generate(s[-1], P[-1], v[-1], 0.0, T[-1] - t_blend[-1], T[-1] + t_blend[-1]) 220 | self.__t = np.concatenate((self.__t, T_Cls.t), dtype=np.float64) 221 | s = np.concatenate((s, s_tmp), dtype=np.float64); s_dot = np.concatenate((s_dot, s_dot_tmp), dtype=np.float64); s_ddot = np.concatenate((s_ddot, s_ddot_tmp), dtype=np.float64) 222 | 223 | # Release of the object. 224 | del T_Cls 225 | 226 | return (s, s_dot, s_ddot) 227 | 228 | def __Generate_Polynomial(self, P: tp.List[tp.List[float]], t_blend: tp.List[float], T: tp.List[float], 229 | v: tp.List[float]) -> tp.Tuple[tp.List[float], 230 | tp.List[float], 231 | tp.List[float]]: 232 | """ 233 | Description: 234 | A function to generate position, velocity, and acceleration of a multi-segment trajectory using 235 | the polynomial trajectories (degree 5 - quintic) method. 236 | 237 | Args: 238 | (1) P [Vector 1xn]: Input control points (waypoints) to be used for trajectory generation. 239 | (2) t_blend [Vector 1xn]: Duration of the blend phase. 240 | (3) T [Vector 1xn]: The velocity of each trajectory segment. 241 | (4) v [Vector 1x(n-1)]: The time of each trajectory segment. 242 | 243 | Returns: 244 | (1 - 3) parameter [Vector 1xN]: Position, velocity and acceleration trapezoidal trajectory. 245 | Note: 246 | Where N is the number of time points of the trajectory. 247 | 248 | Note: 249 | The variable n equals the number of points. 250 | """ 251 | 252 | # Initialization of the class to generate trajectory using a linear function. 253 | L_Cls = Lib.Trajectory.Utilities.Linear_Function_Cls(self.__delta_time) 254 | 255 | # Initialization of the class to generate trajectory using using a polynomial 256 | # profile of degree 5 (quintic). 257 | P_Cls = Lib.Trajectory.Utilities.Polynomial_Profile_Cls(self.__delta_time) 258 | 259 | # Phase 1: The trajectory starts. 260 | (s_tmp, s_dot_tmp, s_ddot_tmp) = P_Cls.Generate(P[0], P[0] + v[0] * t_blend[0], 0.0, v[0], 0.0, 0.0, T[0] - t_blend[0], T[0] + t_blend[0]) 261 | self.__t = P_Cls.t 262 | s = s_tmp; s_dot = s_dot_tmp; s_ddot = s_ddot_tmp 263 | 264 | # Phase 2: The trajectory alternates between linear and blend phases. 265 | for _, (P_ii, v_i, v_ii, T_i, T_ii, t_blend_i, t_blend_ii) in enumerate(zip(P[1::], v, v[1::], T, T[1::], t_blend, t_blend[1::])): 266 | # Linear phase. 267 | (s_tmp, s_dot_tmp, s_ddot_tmp) = L_Cls.Generate(s[-1], v_i, T_i + t_blend_i, T_ii - t_blend_ii) 268 | self.__t = np.concatenate((self.__t, L_Cls.t), dtype=np.float64) 269 | s = np.concatenate((s, s_tmp), dtype=np.float64); s_dot = np.concatenate((s_dot, s_dot_tmp), dtype=np.float64); s_ddot = np.concatenate((s_ddot, s_ddot_tmp), dtype=np.float64) 270 | 271 | # Polynomial blends phase. 272 | (s_tmp, s_dot_tmp, s_ddot_tmp) = P_Cls.Generate(s[-1], P_ii + v_ii * t_blend_ii, v_i, v_ii, 0.0, 0.0, T_ii - t_blend_ii, T_ii + t_blend_ii) 273 | self.__t = np.concatenate((self.__t, P_Cls.t), dtype=np.float64) 274 | s = np.concatenate((s, s_tmp), dtype=np.float64); s_dot = np.concatenate((s_dot, s_dot_tmp), dtype=np.float64); s_ddot = np.concatenate((s_ddot, s_ddot_tmp), dtype=np.float64) 275 | 276 | # Phase 3: The trajectory ends. 277 | # Linear phase. 278 | (s_tmp, s_dot_tmp, s_ddot_tmp) = L_Cls.Generate(s[-1], v[-1], T[-2] + t_blend[-2], T[-1] - t_blend[-1]) 279 | self.__t = np.concatenate((self.__t, L_Cls.t), dtype=np.float64) 280 | s = np.concatenate((s, s_tmp), dtype=np.float64); s_dot = np.concatenate((s_dot, s_dot_tmp), dtype=np.float64); s_ddot = np.concatenate((s_ddot, s_ddot_tmp), dtype=np.float64) 281 | # Polynomial blends phase. 282 | (s_tmp, s_dot_tmp, s_ddot_tmp) = P_Cls.Generate(s[-1], P[-1], v[-1], 0.0, 0.0, 0.0, T[-1] - t_blend[-1], T[-1] + t_blend[-1]) 283 | self.__t = np.concatenate((self.__t, P_Cls.t), dtype=np.float64) 284 | s = np.concatenate((s, s_tmp), dtype=np.float64); s_dot = np.concatenate((s_dot, s_dot_tmp), dtype=np.float64); s_ddot = np.concatenate((s_ddot, s_ddot_tmp), dtype=np.float64) 285 | 286 | # Release of the object. 287 | del L_Cls; del P_Cls 288 | 289 | return (s, s_dot, s_ddot) 290 | 291 | def Generate(self, P: tp.List[tp.List[float]], delta_T: tp.List[float], t_blend: tp.List[float]) -> tp.Tuple[tp.List[float], 292 | tp.List[float], 293 | tp.List[float], 294 | tp.List[float], 295 | float]: 296 | """ 297 | Description: 298 | A function to generate position, velocity, and acceleration of a multi-segment trajectory using 299 | the selected method. 300 | 301 | Args: 302 | (1) P [Vector 1xn]: Input control points (waypoints) to be used for trajectory generation. 303 | (2) delta_T [Vector 1x(n-1)]: Trajectory duration between control points. 304 | (3) t_blend [Vector 1xn]: Duration of the blend phase. 305 | 306 | Returns: 307 | (1 - 3) parameter [Vector 1xN]: Position, velocity and acceleration trapezoidal trajectory. 308 | Note: 309 | Where N is the number of time points of the trajectory. 310 | (4) parameter [Vector 1xn]: The time of each trajectory segment. 311 | Note: 312 | when the trajectory reaches the control points 313 | (5) parameter [float]: The arc length L(t) of the position part of the trajectory. 314 | 315 | Note: 316 | The variable n equals the number of points. 317 | """ 318 | 319 | try: 320 | assert P.size == t_blend.size and P.size == delta_T.size + 1 321 | 322 | # Express the velocity of each trajectory segment. 323 | v = np.zeros(delta_T.size, dtype=np.float64) 324 | for i, (P_i, P_ii, delta_T_i) in enumerate(zip(P, P[1::], delta_T)): 325 | v[i] = (P_ii - P_i)/delta_T_i 326 | 327 | # Express the time of each trajectory segment. 328 | T = np.zeros(P.size, dtype=np.float64) 329 | for i, delta_T_i in enumerate(delta_T, start=1): 330 | T[i] = T[i-1] + delta_T_i 331 | 332 | if self.__method == 'Trapezoidal': 333 | (s, s_dot, s_ddot) = self.__Generate_Trapezoidal(P, t_blend, T, v) 334 | else: 335 | (s, s_dot, s_ddot) = self.__Generate_Polynomial(P, t_blend, T, v) 336 | 337 | return (s, s_dot, s_ddot, T, self.__Get_Arc_Length(s_dot)) 338 | 339 | except AssertionError as error: 340 | print(f'[ERROR] Information: {error}') 341 | print('[ERROR] Incorrect size of function input parameters.') 342 | print('[ERROR] The expected size of the input parameters must be of the form P(1, n), t(1, n-1) and t_blend(1, n).') 343 | -------------------------------------------------------------------------------- /src/Lib/Trajectory/Utilities.py: -------------------------------------------------------------------------------- 1 | """ 2 | ## =========================================================================== ## 3 | MIT License 4 | Copyright (c) 2023 Roman Parak 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | The above copyright notice and this permission notice shall be included in all 12 | copies or substantial portions of the Software. 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. 20 | ## =========================================================================== ## 21 | Author : Roman Parak 22 | Email : Roman.Parak@outlook.com 23 | Github : https://github.com/rparak 24 | File Name: Utilities.py 25 | ## =========================================================================== ## 26 | """ 27 | 28 | # Numpy (Array computing) [pip3 install numpy] 29 | import numpy as np 30 | # Typing (Support for type hints) 31 | import typing as tp 32 | 33 | class Linear_Function_Cls(object): 34 | """ 35 | Description: 36 | The specific class for generating a linear function from input constraints. 37 | 38 | Initialization of the Class: 39 | Args: 40 | (1) delta_time [float]: The difference (spacing) between the time values. 41 | 42 | Example: 43 | Initialization: 44 | # Assignment of the variables. 45 | delta_time = 0.01 46 | 47 | # Initialization of the class. 48 | Cls = Linear_Function_Cls(delta_time) 49 | 50 | Features: 51 | # Properties of the class. 52 | Cls.t 53 | ... 54 | Cls.N 55 | 56 | # Functions of the class. 57 | Cls.Generate(1.57, 0.0, 0.0, 0.0, 0.0) 58 | """ 59 | 60 | def __init__(self, delta_time: float) -> None: 61 | # The difference (spacing) between the time values. 62 | self.__delta_time = delta_time 63 | 64 | @property 65 | def t(self) -> tp.List[float]: 66 | """ 67 | Description: 68 | Get evenly spaced time values at the following interval, see below: 69 | t_0 <= t <= t_f 70 | 71 | Returns: 72 | (1) parameter [Vector 1xn]: Time values. 73 | Note: 74 | Where n is the number of time values. 75 | """ 76 | 77 | return self.__t 78 | 79 | @property 80 | def N(self) -> int: 81 | """ 82 | Description: 83 | Get the number of time points of the trajectory. 84 | 85 | Returns: 86 | (1) parameter [int]: Number of time points. 87 | """ 88 | 89 | return self.__t.size 90 | 91 | @property 92 | def delta_time(self) -> float: 93 | """ 94 | Description: 95 | Get the difference (spacing) between the time values. 96 | 97 | Returns: 98 | (1) parameter [float]: The difference (spacing) between the time values. 99 | """ 100 | 101 | return self.__delta_time 102 | 103 | def Generate(self, s_0: float, v_0: float, t_0: float, t_f: float) -> tp.Tuple[tp.List[float], 104 | tp.List[float], 105 | tp.List[float]]: 106 | """ 107 | Description: 108 | A function to generate position, velocity, and acceleration of linear trajectories. 109 | 110 | Args: 111 | (1, 2) s_0, v_0 [float]: Configuration of position (s) and velocity (v) constraints. 112 | (3, 4) t_0, t_f [float]: Initial and final time constraints. 113 | 114 | Returns: 115 | (1 - 3) parameter [Vector 1xN]: Position, velocity and acceleration polynomial trajectory of degree 5. 116 | Note: 117 | Where N is the number of time points of the trajectory. 118 | """ 119 | 120 | # Get evenly distributed time values in a given interval. 121 | # t_0 <= t <= t_f 122 | self.__t = np.arange(t_0, t_f + self.__delta_time, self.__delta_time) 123 | 124 | # Express the position (s), velocity (s_dot), and acceleration (s_ddot) of a linear 125 | # trajectory. 126 | # Equation (polynomial form): 127 | # s(t) = s_0 + v_0 * (t - t_0) 128 | s = np.array(s_0 + v_0 * (self.__t - t_0), dtype=np.float64) 129 | # Note: 130 | # The velocity is equal to the input velocity vector. 131 | s_dot = np.ones(self.__t.size, dtype=np.float64) * v_0 132 | # Note: 133 | # The acceleration is equal to the vector of zeros. 134 | s_ddot = np.zeros(self.__t.size, dtype=np.float64) 135 | 136 | return (s, s_dot, s_ddot) 137 | 138 | class Trapezoidal_Profile_Cls(object): 139 | """ 140 | Description: 141 | The specific class for generating the trapezoidal trajectory from input constraints. 142 | 143 | Initialization of the Class: 144 | Args: 145 | (1) delta_time [float]: The difference (spacing) between the time values. 146 | 147 | Example: 148 | Initialization: 149 | # Assignment of the variables. 150 | delta_time = 0.01 151 | 152 | # Initialization of the class. 153 | Cls = Trapezoidal_Profile_Cls(delta_time) 154 | 155 | Features: 156 | # Properties of the class. 157 | Cls.t 158 | ... 159 | Cls.N 160 | 161 | # Functions of the class. 162 | Cls.Generate(0.0, 1.57, 0.0, 0.0, 163 | 0.0, 1.0) 164 | """ 165 | 166 | def __init__(self, delta_time: float) -> None: 167 | # The difference (spacing) between the time values. 168 | self.__delta_time = delta_time 169 | 170 | @property 171 | def t(self) -> tp.List[float]: 172 | """ 173 | Description: 174 | Get evenly spaced time values at the following interval, see below: 175 | t_0 <= t <= t_f 176 | 177 | Returns: 178 | (1) parameter [Vector 1xn]: Time values. 179 | Note: 180 | Where n is the number of time values. 181 | """ 182 | 183 | return self.__t 184 | 185 | @property 186 | def N(self) -> int: 187 | """ 188 | Description: 189 | Get the number of time points of the trajectory. 190 | 191 | Returns: 192 | (1) parameter [int]: Number of time points. 193 | """ 194 | 195 | return self.__t.size 196 | 197 | @property 198 | def delta_time(self) -> float: 199 | """ 200 | Description: 201 | Get the difference (spacing) between the time values. 202 | 203 | Returns: 204 | (1) parameter [float]: The difference (spacing) between the time values. 205 | """ 206 | 207 | return self.__delta_time 208 | 209 | def __Method_Null_Intial_Velocities(self, s_0: float, s_f: float, t_0: float, t_f: float) -> tp.Tuple[tp.List[float], 210 | tp.List[float], 211 | tp.List[float]]: 212 | """ 213 | Description: 214 | A function to generate position, velocity, and acceleration trapezoidal trajectories with null initial 215 | and final velocities. 216 | 217 | Args: 218 | (1, 2) s_0, s_f [float]: Initial and final configuration of position constraints. 219 | (3, 4) t_0, t_f [float]: Initial and final time constraints. 220 | 221 | Returns: 222 | (1 - 3) parameter [Vector 1xn]: Position, velocity and acceleration trapezoidal trajectory. 223 | """ 224 | 225 | # Get evenly distributed time values in a given interval. 226 | # t_0 <= t <= t_f 227 | t = np.arange(t_0, t_f + self.__delta_time, self.__delta_time) 228 | 229 | if t_0 == 0.0: 230 | self.__t = t.copy() 231 | else: 232 | # If time t_0 is non-null, normalize the time. 233 | # 0.0 <= t <= 1.0 234 | self.__t = np.linspace(0.0, 1.0, t.size) 235 | 236 | # Express the last value of the time. 237 | t_f = self.__t[-1] 238 | 239 | # Initialization of the output varliables. 240 | s = np.zeros(self.N, dtype=np.float64) 241 | s_dot = s.copy(); s_ddot = s.copy() 242 | 243 | # Calculate the velocity automatically. 244 | # Note: 245 | # The velocity must be within the interval, see below: 246 | # (s_f - s_0) / t_f < v < 2 * ((s_f - s_0) / t_f) 247 | v = 1.5 * ((s_f - s_0) / t_f) 248 | 249 | # Time of constant acceleration phase. 250 | T_a = ((s_0 - s_f) + v * t_f) / v 251 | 252 | # Express the acceleration with a simple formula. 253 | a = v / T_a 254 | 255 | # Express the position (s), velocity (s_dot), and acceleration (s_ddot) of a trapezoidal 256 | # trajectory. 257 | for i, t_i in enumerate(self.__t): 258 | if t_i <= T_a: 259 | # Phase 1: Acceleration. 260 | # t -> [0.0, T_a] 261 | s[i] = s_0 + 0.5 * a * t_i**2 262 | s_dot[i] = a * t_i 263 | s_ddot[i] = a 264 | elif t_i <= t_f - T_a: 265 | # Phase 2: Constant velocity. 266 | # t -> [T_a, t_f - T_a] 267 | s[i] = s_0 - 0.5 * v * T_a + v * t_i 268 | s_dot[i] = v 269 | s_ddot[i] = 0.0 270 | elif t_i <= t_f: 271 | # t -> [t_f - T_a, t_f] 272 | # Phase 3: Deceleration. 273 | s[i] = s_f - 0.5 * a * t_f**2 + a * t_f * t_i - 0.5 * a * t_i**2 274 | s_dot[i] = a * t_f - a * t_i 275 | s_ddot[i] = -a 276 | 277 | if t_0 != 0.0: 278 | self.__t = t.copy() 279 | 280 | return (s, s_dot, s_ddot) 281 | 282 | def __Method_Non_Null_Intial_Velocities(self, s_0: float, s_f: float, v_0: float, v_f: float, t_0: float, t_f: float) -> tp.Tuple[tp.List[float], 283 | tp.List[float], 284 | tp.List[float]]: 285 | """ 286 | Description: 287 | A function to generate position, velocity, and acceleration trapezoidal trajectories with non-null initial 288 | and final velocities. 289 | 290 | Args: 291 | (1, 2) s_0, s_f [float]: Initial and final configuration of position constraints. 292 | (3, 4) v_0, v_f [float]: Initial and final configuration of velocity constraints. 293 | (5, 6) t_0, t_f [float]: Initial and final time constraints. 294 | 295 | Returns: 296 | (1 - 3) parameter [Vector 1xn]: Position, velocity and acceleration trapezoidal trajectory. 297 | """ 298 | 299 | # Get evenly distributed time values in a given interval. 300 | # t_0 <= t <= t_f 301 | self.__t = np.arange(t_0, t_f + self.__delta_time, self.__delta_time) 302 | 303 | # Initialization of the output varliables. 304 | s = np.zeros(self.N, dtype=np.float64) 305 | s_dot = s.copy(); s_ddot = s.copy() 306 | 307 | # Express the velocity with a simple formula. 308 | v = (v_f - v_0) 309 | 310 | # Time of constant acceleration phase. 311 | T_a = (t_f - t_0) 312 | 313 | # Express the acceleration with a simple formula. 314 | a = v / T_a 315 | 316 | # Express the position (s), velocity (s_dot), and acceleration (s_ddot) of a trapezoidal 317 | # trajectory. 318 | for i, t_i in enumerate(self.__t): 319 | t = (t_i - t_0) 320 | if t_0 < t_0 + T_a: 321 | # Phase 1: Acceleration. 322 | # t -> [t_0, t0 + T_a] 323 | s[i] = s_0 + v_0*t + 0.5 * a * t ** 2 324 | s_dot[i] = v_0 + a*t 325 | s_ddot[i] = a 326 | elif t_0 + T_a < t_f - T_a: 327 | # Phase 2: Constant velocity. 328 | # t -> [t_0 + T_a, t_f − T_a] 329 | s[i] = s_0 + v_0 * 0.5 * T_a + v * (t - t_0 - 0.5 * T_a) 330 | s_dot[i] = v 331 | s_ddot[i] = 0.0 332 | elif t_f - T_a <= t_f: 333 | # Phase 3: Deceleration. 334 | # t -> [t_f − T_a, t_f] 335 | s[i] = s_f - v_f * (t_f - t) - ((v - v_f)/(2*T_a)) * (t_f - t)**2 336 | s_dot[i] = v_f + ((v - v_f)/(T_a)) * (t_f - t) 337 | s_ddot[i] = -a 338 | 339 | return (s, s_dot, s_ddot) 340 | 341 | def Generate(self, s_0: float, s_f: float, v_0: float, v_f: float, t_0: float, t_f: float) -> tp.Tuple[tp.List[float], 342 | tp.List[float], 343 | tp.List[float]]: 344 | """ 345 | Description: 346 | A function to generate position, velocity, and acceleration of trapezoidal trajectories with non-null initial 347 | and final velocities. 348 | 349 | Args: 350 | (1, 2) s_0, s_f [float]: Initial and final configuration of position constraints. 351 | (3, 4) v_0, v_f [float]: Initial and final configuration of velocity constraints. 352 | (5, 6) t_0, t_f [float]: Initial and final time constraints. 353 | 354 | Returns: 355 | (1 - 3) parameter [Vector 1xN]: Position, velocity and acceleration trapezoidal trajectory. 356 | Note: 357 | Where N is the number of time points of the trajectory. 358 | """ 359 | 360 | if v_0 == 0.0 and v_f == 0.0: 361 | return self.__Method_Null_Intial_Velocities(s_0, s_f, t_0, t_f) 362 | else: 363 | return self.__Method_Non_Null_Intial_Velocities(s_0, s_f, v_0, v_f, 364 | t_0, t_f) 365 | 366 | class Polynomial_Profile_Cls(object): 367 | """ 368 | Description: 369 | The specific class for generating the polynomial trajectory of degree 5 (quintic) from input constraints. 370 | 371 | Initialization of the Class: 372 | Args: 373 | (1) delta_time [float]: The difference (spacing) between the time values. 374 | 375 | Example: 376 | Initialization: 377 | # Assignment of the variables. 378 | delta_time = 0.01 379 | 380 | # Initialization of the class. 381 | Cls = Polynomial_Profile_Cls(delta_time) 382 | 383 | Features: 384 | # Properties of the class. 385 | Cls.t 386 | ... 387 | Cls.N 388 | 389 | # Functions of the class. 390 | Cls.Generate(0.0, 1.57, 0.0, 0.0, 0.0, 0.0, 391 | 0.0, 1.0) 392 | """ 393 | 394 | def __init__(self, delta_time: float) -> None: 395 | # The difference (spacing) between the time values. 396 | self.__delta_time = delta_time 397 | 398 | @property 399 | def t(self) -> tp.List[float]: 400 | """ 401 | Description: 402 | Get evenly spaced time values at the following interval, see below: 403 | t_0 <= t <= t_f 404 | 405 | Returns: 406 | (1) parameter [Vector 1xn]: Time values. 407 | Note: 408 | Where n is the number of time values. 409 | """ 410 | 411 | return self.__t 412 | 413 | @property 414 | def N(self) -> int: 415 | """ 416 | Description: 417 | Get the number of time points of the trajectory. 418 | 419 | Returns: 420 | (1) parameter [int]: Number of time points. 421 | """ 422 | 423 | return self.__t.size 424 | 425 | @property 426 | def delta_time(self) -> float: 427 | """ 428 | Description: 429 | Get the difference (spacing) between the time values. 430 | 431 | Returns: 432 | (1) parameter [float]: The difference (spacing) between the time values. 433 | """ 434 | 435 | return self.__delta_time 436 | 437 | def __Quintic_Polynomial(self, t_0: float, t_f: float) -> tp.List[tp.List[float]]: 438 | """ 439 | Descrtiption: 440 | Obtain the modified polynomial matrix of degree 5. 441 | 442 | A polynomial of degree 5 (quintic) is defined as follows: 443 | s(t) = c_{0} + c_{1}*t + c_{2}*t^2 + c_{3}*t^3 + c_{4}*t^4 + c_{5}*t^5 444 | 445 | The quintic polynomial can be expressed by a system of 6 equations. These equations 446 | can be converted into a matrix, which we have called X. 447 | 448 | X = [[1.0, t_0, t_0**2, t_0**3, t_0**4, t_0**5], 449 | [0.0, 1.0, 2.0 * t_0, 3.0 * t_0**2, 4.0 * t_0**3, 5.0 * t_0**4], 450 | [0.0, 0.0, 2.0, 6.0 * t_0, 12.0 * t_0**2, 20.0 * t_0**3], 451 | [1.0, t_f, t_f**2, t_f**3, t_f**4, t_f**5], 452 | [0.0, 1.0, 2.0 * t_f, 3.0 * t_f**2, 4.0 * t_f**3, 5.0 * t_f**4], 453 | [0.0, 0.0, 2.0, 6.0 * t_f, 12.0 * t_f**2, 20.0 * t_f**3]] 454 | 455 | Args: 456 | (1, 2) t_0, t_f [float]: Initial and final time constant. 457 | 458 | Returns: 459 | (1) parameter [Vector 6x6]: Modified polynomial matrix of degree 5. 460 | """ 461 | 462 | return np.array([[1.0, t_0, t_0**2, t_0**3, t_0**4, t_0**5], 463 | [0.0, 1.0, 2.0 * t_0, 3.0 * t_0**2, 4.0 * t_0**3, 5.0 * t_0**4], 464 | [0.0, 0.0, 2.0, 6.0 * t_0, 12.0 * t_0**2, 20.0 * t_0**3], 465 | [1.0, t_f, t_f**2, t_f**3, t_f**4, t_f**5], 466 | [0.0, 1.0, 2.0 * t_f, 3.0 * t_f**2, 4.0 * t_f**3, 5.0 * t_f**4], 467 | [0.0, 0.0, 2.0, 6.0 * t_f, 12.0 * t_f**2, 20.0 * t_f**3]], dtype=np.float64) 468 | 469 | def Generate(self, s_0: float, s_f: float, v_0: float, v_f: float, a_0: float, a_f: float, t_0: float, t_f: float) -> tp.Tuple[tp.List[float], 470 | tp.List[float], 471 | tp.List[float]]: 472 | """ 473 | Description: 474 | A function to generate position, velocity, and acceleration of polynomial trajectories (degree 5 - quintic). 475 | 476 | Args: 477 | (1, 2) s_0, s_f [float]: Initial and final configuration of position constraints. 478 | (3, 4) v_0, v_f [float]: Initial and final configuration of velocity constraints. 479 | (5, 6) a_0, a_f [float]: Initial and final configuration of acceleration constraints. 480 | (7, 8) t_0, t_f [float]: Initial and final time constraints. 481 | 482 | Returns: 483 | (1 - 3) parameter [Vector 1xN]: Position, velocity and acceleration polynomial trajectory of degree 5. 484 | Note: 485 | Where N is the number of time points of the trajectory. 486 | """ 487 | 488 | # Get evenly distributed time values in a given interval. 489 | # t_0 <= t <= t_f 490 | self.__t = np.arange(t_0, t_f + self.__delta_time, self.__delta_time) 491 | 492 | # Obtain the modified polynomial matrix of degree 5. 493 | self.__X = self.__Quintic_Polynomial(t_0, t_f) 494 | 495 | # Initialization of the output varliables. 496 | s = np.zeros(self.__t.size, dtype=np.float64) 497 | s_dot = s.copy(); s_ddot = s.copy() 498 | 499 | # Find the coefficients c_{0 .. 5} from the equation below. 500 | # Equation: 501 | # [c_{0 .. 5}] = X^(-1) * [s_0, s_f, s_dot_0(v_0), s_dot_f(v_f), s_ddot_0(a_0), s_ddot_f(a_f)] 502 | C = np.linalg.inv(self.__X) @ np.append([s_0, v_0, a_0], [s_f, v_f, a_f]) 503 | 504 | # Analytic expression (position): 505 | # s(t) = c_{0} + c_{1}*t + c_{2}*t^2 + c_{3}*t^3 + c_{4}*t^4 + c_{5}*t^5 506 | for i, C_i in enumerate(C): 507 | s[:] += (self.__t ** i) * C_i 508 | 509 | # Analytic expression (velocity): 510 | # s_dot(t) = c_{1} + 2*c_{2}*t + 3*c_{3}*t^2 + 4*c_{4}*t^3 + 5*c_{5}*t^4 511 | for i, (C_ii, var_i) in enumerate(zip(C[1:], np.array([1.0, 2.0, 3.0, 4.0, 5.0], dtype=np.float64)), 512 | start=1): 513 | s_dot[:] += (self.__t ** (i - 1)) * C_ii * var_i 514 | 515 | # Analytic expression (acceleration): 516 | # s_ddot(t) = 2*c_{2}*t + 6*c_{3}*t^2 + 12*c_{4}*t^3 + 20*c_{5}*t^4 517 | for i, (C_iii, var_i) in enumerate(zip(C[2:], np.array([2.0, 6.0, 12.0, 20.0], dtype=np.float64)), 518 | start=2): 519 | s_ddot[:] += (self.__t ** (i - 2)) * C_iii * var_i 520 | 521 | return (s, s_dot, s_ddot) -------------------------------------------------------------------------------- /src/Lib/Transformation/Utilities/Mathematics.py: -------------------------------------------------------------------------------- 1 | """ 2 | ## =========================================================================== ## 3 | MIT License 4 | Copyright (c) 2023 Roman Parak 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | The above copyright notice and this permission notice shall be included in all 12 | copies or substantial portions of the Software. 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. 20 | ## =========================================================================== ## 21 | Author : Roman Parak 22 | Email : Roman.Parak@outlook.com 23 | Github : https://github.com/rparak 24 | File Name: Mathematics.py 25 | ## =========================================================================== ## 26 | """ 27 | 28 | # Numpy (Array computing) [pip3 install numpy] 29 | import numpy as np 30 | # Typing (Support for type hints) 31 | import typing as tp 32 | 33 | """ 34 | Description: 35 | Initialization of constants. 36 | """ 37 | CONST_NULL = 0.0 38 | # The difference between 1.0 and the next smallest 39 | # representable float larger than 1.0. 40 | # Machine epsilon (IEEE-754 standard): b ** (-(p - 1)) 41 | # binary32: b (Base) = 2, p (Precision) = 24 42 | # binary64: b (Base) = 2, p (Precision) = 53 43 | CONST_EPS_32 = 1.19e-07 44 | CONST_EPS_64 = 2.22e-16 45 | # Mathematical constants 46 | CONST_MATH_PI = 3.141592653589793 47 | CONST_MATH_HALF_PI = 1.5707963267948966 48 | 49 | def Radian_To_Degree(x: tp.Union[float, tp.List[float]]) -> tp.Union[float, tp.List[float]]: 50 | """ 51 | Description: 52 | Functions for converting angles from radians to degrees. 53 | 54 | Args: 55 | (1) x [float or Vector]: Angle in radians. 56 | 57 | Returns: 58 | (1) parameter [float or Vector]: Angle in degrees. 59 | """ 60 | 61 | try: 62 | assert isinstance(x, float) or isinstance(x, np.ndarray) 63 | 64 | return x * (180.0 / CONST_MATH_PI) 65 | 66 | except AssertionError as error: 67 | print(f'[ERROR] Information: {error}') 68 | print(f'[ERROR] Incorrect type of input parameters. Input parameter x must be of type float or np.ndarray.') 69 | 70 | def Degree_To_Radian(x: tp.Union[float, tp.List[float]]) -> tp.Union[float, tp.List[float]]: 71 | """ 72 | Description: 73 | Functions for converting angles from degrees to radians. 74 | 75 | Args: 76 | (1) x [float or Vector]: Angle in degrees. 77 | 78 | Returns: 79 | (1) parameter [float or Vector]: Angle in radians. 80 | """ 81 | 82 | try: 83 | assert isinstance(x, float) or isinstance(x, np.ndarray) 84 | 85 | return x * (CONST_MATH_PI / 180.0) 86 | 87 | except AssertionError as error: 88 | print(f'[ERROR] Information: {error}') 89 | print(f'[ERROR] Incorrect type of input parameters. Input parameter x must be of type float or np.ndarray.') 90 | 91 | def Sign(x: float) -> float: 92 | """ 93 | Description: 94 | Function to indicate the sign of a number. 95 | 96 | Note: 97 | The signum function obtains the sign of a real number by mapping 98 | a set of real numbers to a set of three real numbers {-1.0, 0.0, 1.0}. 99 | 100 | Args: 101 | (1) x [float]: Input real number. 102 | 103 | Returns: 104 | (1) parameter [float]: Signum of the input number. 105 | """ 106 | 107 | return [np.abs(x) / x if x != 0.0 else 0.0] 108 | 109 | def Euclidean_Norm(x: tp.List[float]) -> float: 110 | """ 111 | Description: 112 | The square root of the sum of the squares of the x-coordinates. 113 | 114 | Equation: 115 | ||x||_{2} = sqrt(x_{1}^2 + ... + x_{n}^2) 116 | 117 | Args: 118 | (1) x [Vector]: Input coordinates. 119 | 120 | Returns: 121 | (1) parameter [float]: Ordinary distance from the origin to the point {x} a consequence of 122 | the Pythagorean theorem. 123 | """ 124 | 125 | x_res = np.array(x).flatten() 126 | if x_res.size == 1: 127 | return (x ** 2) ** (0.5) 128 | else: 129 | x_res_norm = 0.0 130 | for _, x_res_i in enumerate(x_res): 131 | x_res_norm += x_res_i ** 2 132 | 133 | return x_res_norm ** (0.5) 134 | 135 | def Max(x: tp.List[float]) -> tp.Tuple[int, float]: 136 | """ 137 | Description: 138 | Function to return the maximum number from the list. 139 | 140 | Args: 141 | (1) x [Vector]: Input list for determining the value. 142 | 143 | Returns: 144 | (1) parameter [int]: Index of the maximum value in the list. 145 | (2) parameter [float]: The maximum value of the list. 146 | """ 147 | 148 | max_x = x[0]; idx = 0 149 | for i, x_i in enumerate(x[1:]): 150 | if x_i > max_x: 151 | max_x = x_i 152 | idx = i + 1 153 | 154 | return (idx, max_x) 155 | 156 | def Min(x: tp.List[float]) -> tp.Tuple[int, float]: 157 | """ 158 | Description: 159 | Function to return the minimum number from the list. 160 | 161 | Args: 162 | (1) in_list [Vector]: Input list for determining the value. 163 | 164 | Returns: 165 | (1) parameter [int]: Index of the minimum value in the list. 166 | (2) parameter [float]: The minimum value of the list. 167 | """ 168 | 169 | min_x = x[0]; idx = 0 170 | for i, x_i in enumerate(x[1:]): 171 | if x_i < min_x: 172 | min_x = x_i 173 | idx = i + 1 174 | 175 | return (idx, min_x) 176 | 177 | def Clamp(x: float, a: float, b: float) -> float: 178 | """ 179 | Description: 180 | Clamp (limit) values with a defined interval. For a given interval, values outside the interval 181 | are clipped to the edges of the interval. 182 | 183 | Equation: 184 | Clamp(x) = max(a, min(x, b)) ∈ [a, b] 185 | 186 | Another variant (slower): 187 | Mathematics.Max([a, Mathematics.Min([x, b])[0]])[0] 188 | 189 | Args: 190 | (1) x [float]: The value to be clamped (preferred value). 191 | (2) a [float]: Maximum allowed value (lower bound). 192 | (3) b [float]: Minimum allowed value (upper bound). 193 | 194 | Returns: 195 | (1) parameter [float]: The value clamped to the range of {a: max} and {b: min}. 196 | """ 197 | 198 | return a if x < a else b if x > b else x 199 | 200 | def Cross(a: float, b: float) -> tp.List[float]: 201 | """ 202 | Description: 203 | Get the cross product of two vectors. 204 | 205 | Equation: 206 | 3D: 207 | The three scalar components of the resulting vector s = s_{1}i + s_{2}j + s_{3}k = a x b 208 | are as follows: 209 | 210 | s_{1} = a_{2}*b_{3} - a_{3}*b_{2} 211 | s_{2} = a_{3}*b_{1} - a_{1}*b_{3} 212 | s_{3} = a_{1}*b_{2} - a_{2}*b_{1} 213 | 2D: 214 | s_{1} = a_{1}*b_{2} - a_{2}*b_{1} 215 | 216 | Args: 217 | (1, 2) a, b [Vector]: Input vectors for cross product calculation. 218 | 219 | Returns: 220 | (1) parameter [Vector3]: The resulting vector axb. 221 | """ 222 | 223 | if a.size == 2: 224 | return np.array(a[0]*b[1] - a[1]*b[0], dtype=a.dtype) 225 | elif a.size == 3: 226 | return np.array([a[1]*b[2] - a[2]*b[1], 227 | a[2]*b[0] - a[0]*b[2], 228 | a[0]*b[1] - a[1]*b[0]], dtype=a.dtype) 229 | 230 | def Swap(a: float, b: float) -> tp.Tuple[float, float]: 231 | """ 232 | Description: 233 | A simple function to swap two numbers. 234 | 235 | Note: 236 | a, b = b, a 237 | 238 | Args: 239 | (1, 2) a, b [float]: Input numbers to be swapped. 240 | 241 | Returns: 242 | (1, 2) parameter [float]: Output swapped numbers. 243 | """ 244 | 245 | return (b, a) 246 | 247 | def Binomial_Coefficient(n: int, k: int) -> int: 248 | """ 249 | Description: 250 | Calculation binomial coofecient C, from pair of integers n >= k >= 0 and is written (n k). The binomial coefficients are the 251 | positive integers that occur as coefficients in the binomial theorem. 252 | 253 | (n k) = n! / (k! * (n - k)!) 254 | ... 255 | 256 | Simplification of the calculation: 257 | (n k) = ((n - k + 1) * (n - k + 2) * ... * (n - 1) * (n)) / (1 * 2 * ... * (k - 1) * k) 258 | 259 | Args: 260 | (1) n [int]: Integer number 1 (numerator) 261 | (2) k [int]: Integer number 2 (denumerator) 262 | 263 | Returns: 264 | (1) parameter [int]: Binomial coofecient C(n k). 265 | """ 266 | 267 | try: 268 | assert (n >= k) 269 | 270 | if k == 0: 271 | return 1 272 | elif k == 1: 273 | return n 274 | else: 275 | C_nk = 1 276 | for i in range(0, k): 277 | C_nk *= (n - i); C_nk /= (i + 1) 278 | 279 | return C_nk 280 | 281 | except AssertionError as error: 282 | print(f'[ERROR] Information: {error}') 283 | print('[ERROR] The input condition for the calculation is not satisfied.') 284 | print(f'[ERROR] The number n ({n}) must be larger than or equal to k ({k}).') 285 | 286 | def Factorial(n: int) -> int: 287 | """ 288 | Description: 289 | The product of all positive integers less than or equal to a given positive integer. 290 | The factorial is denoted by {n!}. 291 | 292 | Formula: 293 | n! = n x (n - 1)! 294 | 295 | For example, 296 | 3! = 3 x 2 x 1 = 6. 297 | 298 | Note: 299 | The value of 0! is 1. 300 | 301 | Args: 302 | (1) n [int]: Input number. 303 | 304 | Returns: 305 | (1) parameter [int]: The factorial of a non-negative integer {n}. 306 | """ 307 | 308 | try: 309 | assert (n >= 0) 310 | 311 | if n == 0: 312 | return 1 313 | else: 314 | out = 1 315 | for i in range(2, n + 1): 316 | out *= i 317 | 318 | return out 319 | 320 | except AssertionError as error: 321 | print(f'[ERROR] Information: {error}') 322 | print('[ERROR] The input condition for the calculation is not satisfied.') 323 | print('[ERROR] The number n ({n}) must be larger than or equal to 0.') 324 | 325 | def Combinations(n: int, r: int): 326 | """ 327 | Description: 328 | Get the number of possible combinations. 329 | 330 | Formula: 331 | C(n,r) = n!/(r! * (n - r)!) 332 | 333 | Args: 334 | (1) n [int]: Total number of objects. 335 | (2) r [int]: Sample size. 336 | 337 | Returns: 338 | (1) parameter [int]: The number of possible combinations. 339 | """ 340 | 341 | return int(Factorial(n) / (Factorial(r) * Factorial(n - r))) 342 | 343 | def Perpendicular_Distance(A: tp.List[float], B: tp.List[float], C: tp.List[float]) -> float: 344 | """ 345 | Description: 346 | A function to calculate the shortest distance (or the perpendicular distance). The perpendicular 347 | distance is the shortest distance between a point {P} and a line {L}. 348 | 349 | Note: 350 | If B != C, the perpendicular distance from A to the line through B and C is: 351 | ||(A - B) x (C - B)|| / ||C - B||, 352 | otherwise, 353 | ||(A - B)||, 354 | where x denotes the vector cross product. 355 | 356 | Args: 357 | (1) A [Vector 1xn]: Point on the line between points B and C. 358 | (2) B [Vector 1xn]: Start point on the line (P_{0}). 359 | (3) C [Vector 1xn]: End point on the line (P_{n}). 360 | 361 | Note: 362 | Where n is the number of dimensions of the point 363 | 364 | Returns: 365 | (1) parameter [float]: Perpendicular distance. 366 | """ 367 | 368 | if (B == C).all() == True: 369 | return Euclidean_Norm((A - B)) 370 | else: 371 | return Euclidean_Norm(Cross((A - B), (C - B)))/Euclidean_Norm((C - B)) 372 | 373 | def Linear_Eqn(a: float, b: float) -> tp.List[float]: 374 | """ 375 | Description: 376 | A function to obtain the roots of a linear equation. 377 | 378 | The equation is defined as follows: 379 | a*x + b = 0.0 -> x = (-b)/(a) 380 | 381 | Args: 382 | (1) a, b [float]: Input values of polynomial coefficients. 383 | Note: 384 | a: Coefficient of x^1. 385 | b: Coefficient of x^0. Constant. 386 | 387 | Returns: 388 | (1) parameter [Vector 1x1]: The root of the linear equation. 389 | """ 390 | 391 | if a != 0.0: 392 | return np.array([-b/a], dtype=a.dtype) 393 | else: 394 | return np.array([0.0], dtype=a.dtype) 395 | 396 | def Quadratic_Eqn(a: float, b: float, c: float) -> tp.List[float]: 397 | """ 398 | Description: 399 | A function to obtain the roots of a quadratic equation. 400 | 401 | The equation is defined as follows: 402 | Solution 1: 403 | a*x^2 + b*x + c = 0.0 -> x = (-b +- sqrt(b^2 - 4*a*c))/(2*a) 404 | 405 | Solution 2: 406 | a + b*(1/x) + c*(1/x^2) = 0.0 -> (2*c)/(-b +- sqrt(b^2 - 4*a*c)) 407 | 408 | where {b^2 - 4*a*c} is discriminant {D}. 409 | 410 | D > 0: 411 | The roots are real and distinct. 412 | D = 0: 413 | The roots are real and equal. 414 | D < 0: 415 | The roots do not exist or the roots are imaginary. 416 | 417 | Args: 418 | (1) a, b, c [float]: Input values of polynomial coefficients. 419 | Note: 420 | a: Coefficient of x^2. 421 | b: Coefficient of x^1. 422 | c: Coefficient of x^0. Constant. 423 | 424 | Returns: 425 | (1) parameter [Vector 1x2]: The roots of the quaratic equation. 426 | """ 427 | 428 | if a != 0.0: 429 | # Calculate the discriminant {D}. 430 | D = b*b - 4*a*c 431 | 432 | if D >= 0.0: 433 | # Numerically stable method for solving quadratic equations. 434 | if b >= 0.0: 435 | return np.array([(-b - (D)**(0.5)) / (2*a), (2*c)/(-b - (D)**(0.5))], dtype=a.dtype) 436 | else: 437 | return np.array([(-b + (D)**(0.5)) / (2*a), (2*c)/(-b + (D)**(0.5))], dtype=a.dtype) 438 | else: 439 | # The roots do not exist or the roots are imaginary. 440 | return np.array([0.0, 0.0], dtype=np.float64) 441 | else: 442 | return np.array([0.0, Linear_Eqn(b, c)[0]], dtype=np.float64) 443 | 444 | def Inverse_Companion_Matrix(p: tp.List[float]) -> tp.List[tp.List[float]]: 445 | """ 446 | Description: 447 | A function to obtain the inverse of the companion matrix {C^(-1)} of polynomial {p} of degree n. 448 | 449 | Formula: 450 | The companion matrix of the monic polynomial 451 | p(x) = x^n + a{n-1} * x^(n-1) + ... + a{1} * x + a{0} 452 | 453 | is the square matrix (n, n) defined as 454 | C(p) = [[0, 0, ..., 0, -a{0}], 455 | [1, 0, ..., 0, -a{1}], 456 | [0, 1, ..., 0, -a{2}], 457 | [., ., . , ., .], 458 | [., ., . , ., .], 459 | [., ., ., ., .], 460 | [0, 0, ..., 1, -a{n-1}]]. 461 | 462 | If a{0} is greater then 0, that the inverse of the companion matrix C is defined as 463 | C^(-1)(p) = [[ -a{1}/a{0}, 1, 0, ..., 0], 464 | [ -a{2}/a{0}, 0, 1, ..., 0], 465 | [ ., ., ., . , .], 466 | [ ., ., ., . , .], 467 | [ ., ., ., ., .], 468 | [-a{n-1}/a{0}, 0, ., ..., 1], 469 | [ -1/a{0}, 0, ., ..., 0]]. 470 | 471 | and the characteristic polynomial is 472 | p(x) = x^n + a{1}/a{0} * x^(n-1) + ... + a{n-1}/a{0} * x + 1/a{0} 473 | 474 | References: 475 | 1. R. A. Horn and C. R. Johnson, "Matrix Analysis". 476 | 2. M. Fiedler, "A note on companion matrices" 477 | 478 | Args: 479 | (1) p [Vector 1xn]: Input values of polynomial coefficients. 480 | Note: 481 | Where {n} is the size of p. 482 | 483 | Returns: 484 | (1) parameter [Matrix (p.size - 1 x p.size - 1)]: Inverse of the companion matrix {C^(-1)} associated 485 | with the polynomial {p}. 486 | """ 487 | 488 | if isinstance(p, list): 489 | p = np.array(p, dtype=np.float64) 490 | 491 | # The size {N} of the square matrix. 492 | N = p.size - 1 493 | 494 | # Create the inverse of the companion matrix: C^(-1) 495 | C = np.zeros((N, N), dtype=p.dtype) 496 | C[:, 0] = (-1) * (p[1:] / p[0]) 497 | C[0:N-1, 1:] = np.eye(N - 1, dtype=p.dtype) 498 | 499 | return C 500 | 501 | def N_Degree_Eqn(p: tp.List[float]) -> tp.List[float]: 502 | """ 503 | Description: 504 | A function to obtain the eigenvalues {lambda} of the inverse of the companion matrix {C^(-1)}, which are defined 505 | as the roots of the characteristic polynomial {p}. 506 | 507 | Formula: 508 | p{0} * x^n + p{1} * x^(n-1) + ... + p{n-1}*x + p{n} 509 | 510 | Args: 511 | (1) p [Vector 1xn]: Input values of polynomial coefficients. 512 | Note: 513 | Where {n} is the size of p. 514 | 515 | Returns: 516 | (1) parameter [Vector 1xn]: The roots of the polynomial of degree n. 517 | """ 518 | 519 | return np.linalg.eigvals(Inverse_Companion_Matrix(p)).real 520 | 521 | def Roots(p: tp.List[float]) -> tp.List[float]: 522 | """ 523 | Description: 524 | A function to obtain the roots of a polynomial of degree n. The function selects the best method 525 | to calculate the roots. 526 | 527 | Methods: 528 | Linear, Quadratic and N-Degree. 529 | 530 | Args: 531 | (1) p [Vector 1xn]: Input values of polynomial coefficients. 532 | Note: 533 | Where {n} is the size of p. 534 | 535 | Returns: 536 | (1) parameter [Vector 1xn]: The roots of the polynomial of degree n. 537 | """ 538 | 539 | try: 540 | if isinstance(p, list): 541 | p = np.array(p, dtype=np.float64) 542 | 543 | # Check the degree of the input polynomial. 544 | assert p.size > 1 545 | 546 | if p.size > 2: 547 | # If the roots cannot be solved using a linear equation. 548 | i = 0 549 | for _, (p_i, p_ii) in enumerate(zip(p, p[1:])): 550 | if p_i == 0 and p_ii != 0: 551 | i += 1 552 | break 553 | 554 | p_n = p[i:] 555 | 556 | # Selects the calculation method based on the number of polynomial coefficients {p}. 557 | return { 558 | 2: lambda x: Linear_Eqn(x[0], x[1]), 559 | 3: lambda x: Quadratic_Eqn(x[0], x[1], x[2]), 560 | 'n': lambda x: N_Degree_Eqn(x) 561 | }[p_n.size if p_n.size < 4 else 'n'](p_n) 562 | else: 563 | return Linear_Eqn(p[0], p[1]) 564 | 565 | except AssertionError as error: 566 | print(f'[ERROR] Information: {error}') 567 | print('[ERROR] Insufficient number of input polynomial coefficients.') 568 | -------------------------------------------------------------------------------- /src/Lib/Utilities/File_IO.py: -------------------------------------------------------------------------------- 1 | """ 2 | ## =========================================================================== ## 3 | MIT License 4 | Copyright (c) 2023 Roman Parak 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | The above copyright notice and this permission notice shall be included in all 12 | copies or substantial portions of the Software. 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. 20 | ## =========================================================================== ## 21 | Author : Roman Parak 22 | Email : Roman.Parak@outlook.com 23 | Github : https://github.com/rparak 24 | File Name: File_IO.py 25 | ## =========================================================================== ## 26 | """ 27 | 28 | # Numpy (Array computing) [pip3 install numpy] 29 | import numpy as np 30 | # Typing (Support for type hints) 31 | import typing as tp 32 | # Pickle (Python object serialization) 33 | import pickle as pkl 34 | 35 | def Load(file_path: str, format: str, separator: str) -> tp.List[tp.Union[float, bool]]: 36 | """ 37 | Description: 38 | A simple function to read data from the file. 39 | 40 | Note: 41 | Deserialization of the data into a binary / text file. 42 | 43 | Args: 44 | (1) file_path [string]: The specified path of the file without extension (format). 45 | (2) format [string]: The format of the loaded file. 46 | Note: 47 | 'pkl' : Pickle file; 'txt' : Text file 48 | (3) separator [string]: Separator between data. 49 | 50 | Returns: 51 | (1) parameter [Dictionary of different types of data ]: Loaded data from a binary file. 52 | """ 53 | 54 | if format == 'pkl': 55 | # Open the file in 'rb' mode (read binary). 56 | with open(file_path + f'.{format}', 'rb') as f: 57 | # Load the data from the file using the file object (f). 58 | data = pkl.load(f) 59 | 60 | elif format == 'txt': 61 | data_tmp = [] 62 | with open(file_path + f'.{format}', 'r') as f: 63 | # Read the line of the file in the current step. 64 | for line in f: 65 | # Splits a string into a list with the specified delimiter (,) 66 | # and converts to a float. 67 | data_tmp.append(np.float64(line.split(separator))) 68 | 69 | # Convert a list to an array. 70 | data = np.array(data_tmp, dtype=np.float64) 71 | 72 | # Close the file after loading the data. 73 | f.close() 74 | 75 | return data 76 | 77 | def Save(file_path: str, data: tp.List[tp.Union[float, bool]], format: str, separator: str) -> None: 78 | """ 79 | Description: 80 | A simple function to write data to the file. 81 | 82 | Note: 83 | Serialization of the data into a binary / text file. 84 | 85 | Args: 86 | (1) file_path [string]: The specified path of the file without extension (format). 87 | (2) data [Dictionary of different types of data ]: Individual data. 88 | (3) format [string]: The format of the saved file. 89 | Note: 90 | 'pkl' : Pickle file; 'txt' : Text file, 'urdf' : Unified Robotics Description Format. 91 | (3) separator [string]: Separator between data. 92 | """ 93 | 94 | if format == 'pkl': 95 | # Open the file in 'wb' mode (write binary). 96 | with open(file_path + f'.{format}', 'wb') as f: 97 | # Write the input data to a file using the file object (f). 98 | pkl.dump(data, f) 99 | elif format in ['txt', 'urdf']: 100 | with open(file_path + f'.{format}', 'a+') as f: 101 | # Write the data to the file. 102 | for data_i in data[:-1]: 103 | f.writelines([str(data_i), separator]) 104 | f.writelines([str(data[-1]), '\n']) 105 | 106 | # Close the file after writing the data. 107 | f.close() --------------------------------------------------------------------------------