├── Articles ├── A General Analytical Algorithm for Collaborative Robot (cobot).pdf ├── Analytic Inverse Kinematics for the Universal Robots.pdf ├── Inverse kinematic solution of 6R robot.pdf ├── Kinematics of a UR5.pdf ├── MULTIPLE CONFIGURATIONS FOR PUNCTURING ROBOT.pdf ├── RealTimeHumanRobotInteractionsandSpeedControlofaRoboticArmforCollaborativeOperations.pdf └── Reza N. Jazar (auth.) - Theory of Applied Robotics_ Kinematics, Dynamics, and Control (2nd Edition)-Springer US (2010).pdf ├── CoppeliaSim files ├── Models │ ├── UR10e.ttm │ ├── Unigripper.ttm │ └── pedestal.stl ├── README.md ├── Scenes │ ├── KinematicsTest.ttt │ ├── UR10.ttt │ ├── UR3.ttt │ ├── UR5.ttt │ └── ValidationTest.ttt └── Scripts │ ├── UR10.lua │ ├── UR3.lua │ └── UR5.lua ├── LAUNCH.md ├── LICENSE ├── MATLAB files ├── GUI.mlapp ├── MDHMatrix.m ├── MatrixOutput │ ├── cMfile.txt │ └── iMfile.txt ├── README.md ├── RPY.m ├── ValidationOutput │ └── stdev.xlsx ├── fwdKin.m ├── invKin8sol.m ├── printM.m ├── remApi.m ├── remoteApi.dll ├── remoteApiProto.m ├── runSim.m └── validationTest.m ├── README.md ├── Resources ├── .gitignore └── errorSolsFlow.png └── cpp └── universalRobotsKinematics ├── .gitignore ├── Application ├── Application.vcxproj ├── Application.vcxproj.filters └── src │ ├── benchmarking.cpp │ ├── benchmarking.h │ ├── coppeliasimTests.cpp │ ├── coppeliasimTests.h │ ├── extApi.c │ ├── extApi.h │ ├── extApiPlatform.c │ ├── extApiPlatform.h │ └── main.cpp ├── Dependencies └── CoppeliaSim │ ├── include │ ├── _dirent.h │ ├── license.txt │ ├── luaFunctionData.h │ ├── luaFunctionDataItem.h │ ├── scriptFunctionData.h │ ├── scriptFunctionDataItem.h │ ├── shared_memory.h │ ├── simConst.h │ ├── simLib.h │ ├── simTypes.h │ ├── socketInConnection.h │ ├── socketOutConnection.h │ └── stack │ │ ├── stackArray.h │ │ ├── stackBool.h │ │ ├── stackMap.h │ │ ├── stackNull.h │ │ ├── stackNumber.h │ │ ├── stackObject.h │ │ └── stackString.h │ └── remoteApi │ ├── extApi.c │ ├── extApi.h │ ├── extApiInternal.h │ ├── extApiPlatform.c │ ├── extApiPlatform.h │ └── license.txt ├── mathLib ├── mathLib.vcxproj ├── mathLib.vcxproj.filters └── src │ ├── mathLib.cpp │ └── mathLib.h ├── universalRobotsKinematics.sln └── universalRobotsKinematics ├── src ├── universalRobotsKinematics.cpp └── universalRobotsKinematics.h ├── universalRobotsKinematics.vcxproj └── universalRobotsKinematics.vcxproj.filters /Articles/A General Analytical Algorithm for Collaborative Robot (cobot).pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/Articles/A General Analytical Algorithm for Collaborative Robot (cobot).pdf -------------------------------------------------------------------------------- /Articles/Analytic Inverse Kinematics for the Universal Robots.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/Articles/Analytic Inverse Kinematics for the Universal Robots.pdf -------------------------------------------------------------------------------- /Articles/Inverse kinematic solution of 6R robot.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/Articles/Inverse kinematic solution of 6R robot.pdf -------------------------------------------------------------------------------- /Articles/Kinematics of a UR5.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/Articles/Kinematics of a UR5.pdf -------------------------------------------------------------------------------- /Articles/MULTIPLE CONFIGURATIONS FOR PUNCTURING ROBOT.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/Articles/MULTIPLE CONFIGURATIONS FOR PUNCTURING ROBOT.pdf -------------------------------------------------------------------------------- /Articles/RealTimeHumanRobotInteractionsandSpeedControlofaRoboticArmforCollaborativeOperations.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/Articles/RealTimeHumanRobotInteractionsandSpeedControlofaRoboticArmforCollaborativeOperations.pdf -------------------------------------------------------------------------------- /Articles/Reza N. Jazar (auth.) - Theory of Applied Robotics_ Kinematics, Dynamics, and Control (2nd Edition)-Springer US (2010).pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/Articles/Reza N. Jazar (auth.) - Theory of Applied Robotics_ Kinematics, Dynamics, and Control (2nd Edition)-Springer US (2010).pdf -------------------------------------------------------------------------------- /CoppeliaSim files/Models/UR10e.ttm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/CoppeliaSim files/Models/UR10e.ttm -------------------------------------------------------------------------------- /CoppeliaSim files/Models/Unigripper.ttm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/CoppeliaSim files/Models/Unigripper.ttm -------------------------------------------------------------------------------- /CoppeliaSim files/Models/pedestal.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/CoppeliaSim files/Models/pedestal.stl -------------------------------------------------------------------------------- /CoppeliaSim files/README.md: -------------------------------------------------------------------------------- 1 | # *CoppeliaSim* Simulation 2 | 3 | ## Scene Setup 4 | 5 | The scene is composed of an UR10 robot model available in *CoppeliaSim* model browser, with a Unigripper Co/Light Regular model attached to its flange. Additionally, the robot is setup on top of a cylindrical pedestal with 0.3 m diameter and 0.7 m height. 6 | 7 | 8 | ![Scene setup](../Images/SceneSetup.png) 9 | 10 | ## Embedded Scripts 11 | 12 | In this scene there is only a threaded child script that is associated to the robot model. This script simply starts the MATLAB API remote connection, retrieves the handles of the robot and its joints, and has a while loop that is executed every five seconds where the robot's tip position is printed in the command window - this is useful to compare with our forward kinematics solution. 13 | 14 | 15 | 16 | ```lua 17 | function sysCall_threadmain() 18 | 19 | --[[ Initialization ]]-- 20 | 21 | --Start MATLAB api 22 | simRemoteApi.start(19999) 23 | 24 | --Handle retreival 25 | local jointHandles={-1,-1,-1,-1,-1,-1} 26 | for i=1,6,1 do 27 | jointHandles[i]=sim.getObjectHandle('UR10_joint'..i) 28 | end 29 | local UR10=sim.getObjectHandle('UR10') 30 | local tip=sim.getObjectHandle('UR10_tip') 31 | 32 | -- Auxiliary variables for outputting current end-effector position 33 | sim.setIntegerSignal('showPos',0) 34 | local IKsol=0; 35 | 36 | --[[ Main loop ]]-- 37 | while sim.getSimulationState()~=sim.simulation_advancing_abouttostop do 38 | if(sim.getIntegerSignal('showPos')==1) then 39 | sim.setIntegerSignal('showPos',0) 40 | -- get tip position relative to the robots' base 41 | local tip_pos=sim.getObjectPosition(tip,UR10) 42 | -- print tip position 43 | IKsol=IKsol+1 44 | print('_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-') 45 | print('End-effector position '.. IKsol) 46 | print(tip_pos) 47 | -- reset value of IKsol, when reaching final solution 48 | if(IKsol==8) then 49 | IKsol=0 50 | end 51 | end 52 | end 53 | 54 | end 55 | ``` -------------------------------------------------------------------------------- /CoppeliaSim files/Scenes/KinematicsTest.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/CoppeliaSim files/Scenes/KinematicsTest.ttt -------------------------------------------------------------------------------- /CoppeliaSim files/Scenes/UR10.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/CoppeliaSim files/Scenes/UR10.ttt -------------------------------------------------------------------------------- /CoppeliaSim files/Scenes/UR3.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/CoppeliaSim files/Scenes/UR3.ttt -------------------------------------------------------------------------------- /CoppeliaSim files/Scenes/UR5.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/CoppeliaSim files/Scenes/UR5.ttt -------------------------------------------------------------------------------- /CoppeliaSim files/Scenes/ValidationTest.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/CoppeliaSim files/Scenes/ValidationTest.ttt -------------------------------------------------------------------------------- /CoppeliaSim files/Scripts/UR10.lua: -------------------------------------------------------------------------------- 1 | function __getObjectPosition__(a,b) 2 | -- compatibility routine, wrong results could be returned in some situations, in CoppeliaSim <4.0.1 3 | if b==sim.handle_parent then 4 | b=sim.getObjectParent(a) 5 | end 6 | if (b~=-1) and (sim.getObjectType(b)==sim.object_joint_type) and (sim.getInt32Parameter(sim.intparam_program_version)>=40001) then 7 | a=a+sim.handleflag_reljointbaseframe 8 | end 9 | return sim.getObjectPosition(a,b) 10 | end 11 | 12 | 13 | function TableConcat(t1,t2) 14 | for i=1,#t2 do 15 | t1[#t1+1] = t2[i] --corrected bug. if t1[#t1+i] is used, indices will be skipped 16 | end 17 | return t1 18 | end 19 | 20 | 21 | function sysCall_threadmain() 22 | 23 | --[[ Initialization ]]-- 24 | 25 | --Start MATLAB api 26 | simRemoteApi.start(19999) 27 | 28 | --Handle retreival 29 | local jointHandles={-1,-1,-1,-1,-1,-1} 30 | for i=1,6,1 do 31 | jointHandles[i]=sim.getObjectHandle('UR10_joint'..i) 32 | end 33 | local UR10=sim.getObjectHandle('UR10') 34 | local tip=sim.getObjectHandle('tip') 35 | 36 | -- Auxiliary variables for outputting current end-effector position 37 | sim.setIntegerSignal('showPos',0) 38 | sim.setStringSignal('tip_pose_final', 'NULL') 39 | local IKsol=0; 40 | 41 | --[[ Main loop ]]-- 42 | while sim.getSimulationState()~=sim.simulation_advancing_abouttostop do 43 | if(sim.getIntegerSignal('showPos')==1) then 44 | sim.setIntegerSignal('showPos',0) 45 | -- get tip pose relative to the robots' base 46 | local tip_pos=__getObjectPosition__(tip,UR10) 47 | local tip_ori_aux=sim.getObjectOrientation(tip,-1) 48 | local tip_ori={-1,-1,-1} 49 | tip_ori[1]=tip_ori_aux[1]*180/math.pi 50 | tip_ori[2]=tip_ori_aux[2]*180/math.pi 51 | tip_ori[3]=tip_ori_aux[3]*180/math.pi 52 | -- print tip pose 53 | IKsol=IKsol+1 54 | print('_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-') 55 | print('Tip pose position '.. IKsol) 56 | print(string.format("{ x = %.3f, y = %.3f, z = %.3f }",tip_pos[1],tip_pos[2],tip_pos[3])) 57 | print(string.format("{ a = %.3f, b = %.3f, g = %.3f }",tip_ori[1],tip_ori[2],tip_ori[3])) 58 | -- send tip pose to Matlab 59 | local packed_tip_pos=sim.packFloatTable(TableConcat(tip_pos,tip_ori)) 60 | sim.setStringSignal('tip_pose_final', packed_tip_pos) 61 | -- reset value of IKsol, when reaching final solution 62 | if(IKsol==8) then 63 | IKsol=0 64 | end 65 | end 66 | end 67 | 68 | end 69 | -------------------------------------------------------------------------------- /CoppeliaSim files/Scripts/UR3.lua: -------------------------------------------------------------------------------- 1 | function __getObjectPosition__(a,b) 2 | -- compatibility routine, wrong results could be returned in some situations, in CoppeliaSim <4.0.1 3 | if b==sim.handle_parent then 4 | b=sim.getObjectParent(a) 5 | end 6 | if (b~=-1) and (sim.getObjectType(b)==sim.object_joint_type) and (sim.getInt32Parameter(sim.intparam_program_version)>=40001) then 7 | a=a+sim.handleflag_reljointbaseframe 8 | end 9 | return sim.getObjectPosition(a,b) 10 | end 11 | 12 | 13 | function TableConcat(t1,t2) 14 | for i=1,#t2 do 15 | t1[#t1+1] = t2[i] --corrected bug. if t1[#t1+i] is used, indices will be skipped 16 | end 17 | return t1 18 | end 19 | 20 | 21 | function sysCall_threadmain() 22 | 23 | --[[ Initialization ]]-- 24 | 25 | --Start MATLAB api 26 | simRemoteApi.start(19999) 27 | 28 | --Handle retreival 29 | local jointHandles={-1,-1,-1,-1,-1,-1} 30 | for i=1,6,1 do 31 | jointHandles[i]=sim.getObjectHandle('UR3_joint'..i) 32 | end 33 | local UR3=sim.getObjectHandle('UR3') 34 | local tip=sim.getObjectHandle('tip') 35 | 36 | -- Auxiliary variables for outputting current end-effector position 37 | sim.setIntegerSignal('showPos',0) 38 | sim.setStringSignal('tip_pose_final', 'NULL') 39 | local IKsol=0; 40 | 41 | --[[ Main loop ]]-- 42 | while sim.getSimulationState()~=sim.simulation_advancing_abouttostop do 43 | if(sim.getIntegerSignal('showPos')==1) then 44 | sim.setIntegerSignal('showPos',0) 45 | -- get tip pose relative to the robots' base 46 | local tip_pos=sim.getObjectPosition(tip,-1) 47 | local tip_ori_aux=sim.getObjectOrientation(tip,-1) 48 | local tip_ori={-1,-1,-1} 49 | tip_ori[1]=tip_ori_aux[1]*180/math.pi 50 | tip_ori[2]=tip_ori_aux[2]*180/math.pi 51 | tip_ori[3]=tip_ori_aux[3]*180/math.pi 52 | -- print tip pose 53 | IKsol=IKsol+1 54 | print('_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-') 55 | print('Tip pose position '.. IKsol) 56 | print(string.format("{ x = %.3f, y = %.3f, z = %.3f }",tip_pos[1],tip_pos[2],tip_pos[3])) 57 | print(string.format("{ a = %.3f, b = %.3f, g = %.3f }",tip_ori[1],tip_ori[2],tip_ori[3])) 58 | -- send tip pose to Matlab 59 | local packed_tip_pos=sim.packFloatTable(TableConcat(tip_pos,tip_ori)) 60 | sim.setStringSignal('tip_pose_final', packed_tip_pos) 61 | -- reset value of IKsol, when reaching final solution 62 | if(IKsol==8) then 63 | IKsol=0 64 | end 65 | end 66 | end 67 | 68 | end 69 | -------------------------------------------------------------------------------- /CoppeliaSim files/Scripts/UR5.lua: -------------------------------------------------------------------------------- 1 | function __getObjectPosition__(a,b) 2 | -- compatibility routine, wrong results could be returned in some situations, in CoppeliaSim <4.0.1 3 | if b==sim.handle_parent then 4 | b=sim.getObjectParent(a) 5 | end 6 | if (b~=-1) and (sim.getObjectType(b)==sim.object_joint_type) and (sim.getInt32Parameter(sim.intparam_program_version)>=40001) then 7 | a=a+sim.handleflag_reljointbaseframe 8 | end 9 | return sim.getObjectPosition(a,b) 10 | end 11 | 12 | 13 | function TableConcat(t1,t2) 14 | for i=1,#t2 do 15 | t1[#t1+1] = t2[i] --corrected bug. if t1[#t1+i] is used, indices will be skipped 16 | end 17 | return t1 18 | end 19 | 20 | 21 | function sysCall_threadmain() 22 | 23 | --[[ Initialization ]]-- 24 | 25 | --Start MATLAB api 26 | simRemoteApi.start(19999) 27 | 28 | --Handle retreival 29 | local jointHandles={-1,-1,-1,-1,-1,-1} 30 | for i=1,6,1 do 31 | jointHandles[i]=sim.getObjectHandle('UR5_joint'..i) 32 | end 33 | local UR5=sim.getObjectHandle('UR5') 34 | local tip=sim.getObjectHandle('tip') 35 | 36 | -- Auxiliary variables for outputting current end-effector position 37 | sim.setIntegerSignal('showPos',0) 38 | sim.setStringSignal('tip_pose_final', 'NULL') 39 | local IKsol=0; 40 | 41 | --[[ Main loop ]]-- 42 | while sim.getSimulationState()~=sim.simulation_advancing_abouttostop do 43 | if(sim.getIntegerSignal('showPos')==1) then 44 | sim.setIntegerSignal('showPos',0) 45 | -- get tip pose relative to the robots' base 46 | local tip_pos=__getObjectPosition__(tip,-1) 47 | local tip_ori_aux=sim.getObjectOrientation(tip,-1) 48 | local tip_ori={-1,-1,-1} 49 | tip_ori[1]=tip_ori_aux[1]*180/math.pi 50 | tip_ori[2]=tip_ori_aux[2]*180/math.pi 51 | tip_ori[3]=tip_ori_aux[3]*180/math.pi 52 | -- print tip pose 53 | IKsol=IKsol+1 54 | print('_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-') 55 | print('Tip pose position '.. IKsol) 56 | print(string.format("{ x = %.3f, y = %.3f, z = %.3f }",tip_pos[1],tip_pos[2],tip_pos[3])) 57 | print(string.format("{ a = %.3f, b = %.3f, g = %.3f }",tip_ori[1],tip_ori[2],tip_ori[3])) 58 | -- send tip pose to Matlab 59 | local packed_tip_pos=sim.packFloatTable(TableConcat(tip_pos,tip_ori)) 60 | sim.setStringSignal('tip_pose_final', packed_tip_pos) 61 | -- reset value of IKsol, when reaching final solution 62 | if(IKsol==8) then 63 | IKsol=0 64 | end 65 | end 66 | end 67 | 68 | end 69 | -------------------------------------------------------------------------------- /LAUNCH.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | # Launching the Simulation 4 | 5 | To execute the simulation launch the *KinematicsTest.ttt* scene and run it. Afterwards, open the *main.m* script, choose the target joint values and run it. You should see the robot move to the eight inverse kinematic solutions and obtain a result similar to the figure below. Comparing the end-effector position calculated in MATLAB we can see it is equal to the value presented in *CoppeliaSim*. Moreover, in the MATLAB command window it is possible to verify that there is one inverse kinematic solution that is equal to the pre-defined target joint values, and that all the other solutions produce the same end-effector position in *CoppeliaSim*. Follow the following link for a [video](https://www.youtube.com/watch?v=8CGWcW34lxo) of simulation execution. 6 | 7 | -------------------------------------------------------------------------------- /MATLAB files/GUI.mlapp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/MATLAB files/GUI.mlapp -------------------------------------------------------------------------------- /MATLAB files/MDHMatrix.m: -------------------------------------------------------------------------------- 1 | %% Modified Denavit-Hartenberg Matrix 2 | 3 | % Transformation matrix from a system of coordinates to another. 4 | 5 | %% Function: MDHMatrix 6 | % 7 | % Computes the transformation matrix from a system of coordinates to 8 | % another. 9 | % 10 | % In: DHparams - Denavit-Hartenberg parameters matrix 11 | % |_ alpha - rotation around the x axis of frame i (deg) 12 | % |_ a - translation along the x axis of frame i (meters) 13 | % |_ d - translation along the z axis of frame i (meters) 14 | % |_ theta - rotation around the z axis of frame i (deg) 15 | % 16 | % Out: 4x4 transformation matrix from frame i-1 to i 17 | %% 18 | 19 | % Use this function to test the program regularly 20 | function T = MDHMatrix(DHparams) 21 | T = [ cosd(DHparams(4)) -sind(DHparams(4)) 0 DHparams(2) ; 22 | sind(DHparams(4))*cosd(DHparams(1)) cosd(DHparams(4))*cosd(DHparams(1)) -sind(DHparams(1)) -sind(DHparams(1))*DHparams(3); 23 | sind(DHparams(4))*sind(DHparams(1)) cosd(DHparams(4))*sind(DHparams(1)) cosd(DHparams(1)) cosd(DHparams(1))*DHparams(3) ; 24 | 0 0 0 1 ]; 25 | end 26 | 27 | % Use this function to print matrices 28 | % function T = MDHMatrix(DHparams) 29 | % T = [ cos(DHparams(4)) -sin(DHparams(4)) 0 DHparams(2) ; 30 | % sin(DHparams(4))*cos(DHparams(1)) cos(DHparams(4))*cos(DHparams(1)) -sin(DHparams(1)) -sin(DHparams(1))*DHparams(3); 31 | % sin(DHparams(4))*sin(DHparams(1)) cos(DHparams(4))*sin(DHparams(1)) cos(DHparams(1)) cos(DHparams(1))*DHparams(3) ; 32 | % 0 0 0 1 ]; 33 | % end -------------------------------------------------------------------------------- /MATLAB files/MatrixOutput/cMfile.txt: -------------------------------------------------------------------------------- 1 | Compound transformation matrices 2 | 0T1 3 | cos(j1),-sin(j1),0,0 4 | sin(j1),cos(j1),0,0 5 | 0,0,1,d1 6 | 0,0,0,1 7 | 0T2 8 | cos(j1)*sin(j2),cos(j1)*cos(j2),-sin(j1),-d2*sin(j1) 9 | sin(j1)*sin(j2),cos(j2)*sin(j1),cos(j1),d2*cos(j1) 10 | cos(j2),-sin(j2),0,d1 11 | 0,0,0,1 12 | 0T3 13 | sin(j2 + j3)*cos(j1),cos(j2 + j3)*cos(j1),-sin(j1),a2*cos(j1)*sin(j2) - d3*sin(j1) - d2*sin(j1) 14 | sin(j2 + j3)*sin(j1),cos(j2 + j3)*sin(j1),cos(j1),d2*cos(j1) + d3*cos(j1) + a2*sin(j1)*sin(j2) 15 | cos(j2 + j3),-sin(j2 + j3),0,d1 + a2*cos(j2) 16 | 0,0,0,1 17 | 0T4 18 | sin(j1 + j2 + j3 + j4)/2 + sin(j2 - j1 + j3 + j4)/2,cos(j1 + j2 + j3 + j4)/2 + cos(j2 - j1 + j3 + j4)/2,-sin(j1),a2*cos(j1)*sin(j2) - d3*sin(j1) - d4*sin(j1) - d2*sin(j1) + a3*cos(j1)*cos(j2)*sin(j3) + a3*cos(j1)*cos(j3)*sin(j2) 19 | cos(j2 - j1 + j3 + j4)/2 - cos(j1 + j2 + j3 + j4)/2,sin(j1 + j2 + j3 + j4)/2 - sin(j2 - j1 + j3 + j4)/2,cos(j1),d2*cos(j1) + d3*cos(j1) + d4*cos(j1) + a2*sin(j1)*sin(j2) + a3*cos(j2)*sin(j1)*sin(j3) + a3*cos(j3)*sin(j1)*sin(j2) 20 | cos(j2 + j3 + j4),-sin(j2 + j3 + j4),0,d1 + a3*cos(j2 + j3) + a2*cos(j2) 21 | 0,0,0,1 22 | 0T5 23 | cos(j1 + j2 + j3 + j4)/2 + cos(j2 - j1 + j3 + j4)/2,- sin(j1 + j2 + j3 + j4)/2 - sin(j2 - j1 + j3 + j4)/2,-sin(j1),a2*cos(j1)*sin(j2) - d3*sin(j1) - d4*sin(j1) - d5*sin(j1) - d2*sin(j1) + a3*cos(j1)*cos(j2)*sin(j3) + a3*cos(j1)*cos(j3)*sin(j2) + a4*cos(j1)*cos(j2)*cos(j3)*sin(j4) + a4*cos(j1)*cos(j2)*cos(j4)*sin(j3) + a4*cos(j1)*cos(j3)*cos(j4)*sin(j2) - a4*cos(j1)*sin(j2)*sin(j3)*sin(j4) 24 | sin(j1 + j2 + j3 + j4)/2 - sin(j2 - j1 + j3 + j4)/2,cos(j1 + j2 + j3 + j4)/2 - cos(j2 - j1 + j3 + j4)/2,cos(j1),d2*cos(j1) + d3*cos(j1) + d4*cos(j1) + d5*cos(j1) + a2*sin(j1)*sin(j2) + a3*cos(j2)*sin(j1)*sin(j3) + a3*cos(j3)*sin(j1)*sin(j2) + a4*cos(j2)*cos(j3)*sin(j1)*sin(j4) + a4*cos(j2)*cos(j4)*sin(j1)*sin(j3) + a4*cos(j3)*cos(j4)*sin(j1)*sin(j2) - a4*sin(j1)*sin(j2)*sin(j3)*sin(j4) 25 | -sin(j2 + j3 + j4),-cos(j2 + j3 + j4),0,d1 + a3*cos(j2 + j3) + a2*cos(j2) + a4*cos(j2 + j3 + j4) 26 | 0,0,0,1 27 | 0T6 28 | cos(j5)*(cos(j1 + j2 + j3 + j4)/2 + cos(j2 - j1 + j3 + j4)/2) - sin(j1)*sin(j5),- cos(j5)*sin(j1) - sin(j5)*(cos(j1 + j2 + j3 + j4)/2 + cos(j2 - j1 + j3 + j4)/2),sin(j1 + j2 + j3 + j4)/2 + sin(j2 - j1 + j3 + j4)/2,a2*cos(j1)*sin(j2) - d3*sin(j1) - d4*sin(j1) - d5*sin(j1) - d2*sin(j1) + a3*cos(j1)*cos(j2)*sin(j3) + a3*cos(j1)*cos(j3)*sin(j2) + a4*cos(j1)*cos(j2)*cos(j3)*sin(j4) + a4*cos(j1)*cos(j2)*cos(j4)*sin(j3) + a4*cos(j1)*cos(j3)*cos(j4)*sin(j2) - a4*cos(j1)*sin(j2)*sin(j3)*sin(j4) 29 | cos(j1)*sin(j5) + cos(j5)*(sin(j1 + j2 + j3 + j4)/2 - sin(j2 - j1 + j3 + j4)/2),cos(j1)*cos(j5) - (sin(j5)*(sin(j1 + j2 + j3 + j4) - sin(j2 - j1 + j3 + j4)))/2,cos(j2 - j1 + j3 + j4)/2 - cos(j1 + j2 + j3 + j4)/2,d2*cos(j1) + d3*cos(j1) + d4*cos(j1) + d5*cos(j1) + a2*sin(j1)*sin(j2) + a3*cos(j2)*sin(j1)*sin(j3) + a3*cos(j3)*sin(j1)*sin(j2) + a4*cos(j2)*cos(j3)*sin(j1)*sin(j4) + a4*cos(j2)*cos(j4)*sin(j1)*sin(j3) + a4*cos(j3)*cos(j4)*sin(j1)*sin(j2) - a4*sin(j1)*sin(j2)*sin(j3)*sin(j4) 30 | - sin(j2 + j3 + j4 + j5)/2 - sin(j2 + j3 + j4 - j5)/2,cos(j2 + j3 + j4 - j5)/2 - cos(j2 + j3 + j4 + j5)/2,cos(j2 + j3 + j4),d1 + a3*cos(j2 + j3) + a2*cos(j2) + a4*cos(j2 + j3 + j4) 31 | 0,0,0,1 32 | 0T7 33 | sin(j1 + j2 + j3 + j4)/2 + sin(j2 - j1 + j3 + j4)/2,cos(j5)*(cos(j1 + j2 + j3 + j4)/2 + cos(j2 - j1 + j3 + j4)/2) - sin(j1)*sin(j5),- cos(j5)*sin(j1) - sin(j5)*(cos(j1 + j2 + j3 + j4)/2 + cos(j2 - j1 + j3 + j4)/2),a2*cos(j1)*sin(j2) - d3*sin(j1) - d4*sin(j1) - d5*sin(j1) - d2*sin(j1) + a3*cos(j1)*cos(j2)*sin(j3) + a3*cos(j1)*cos(j3)*sin(j2) + a4*cos(j1)*cos(j2)*cos(j3)*sin(j4) + a4*cos(j1)*cos(j2)*cos(j4)*sin(j3) + a4*cos(j1)*cos(j3)*cos(j4)*sin(j2) - a4*cos(j1)*sin(j2)*sin(j3)*sin(j4) 34 | cos(j2 - j1 + j3 + j4)/2 - cos(j1 + j2 + j3 + j4)/2,cos(j1)*sin(j5) + cos(j5)*(sin(j1 + j2 + j3 + j4)/2 - sin(j2 - j1 + j3 + j4)/2),cos(j1)*cos(j5) - (sin(j5)*(sin(j1 + j2 + j3 + j4) - sin(j2 - j1 + j3 + j4)))/2,d2*cos(j1) + d3*cos(j1) + d4*cos(j1) + d5*cos(j1) + a2*sin(j1)*sin(j2) + a3*cos(j2)*sin(j1)*sin(j3) + a3*cos(j3)*sin(j1)*sin(j2) + a4*cos(j2)*cos(j3)*sin(j1)*sin(j4) + a4*cos(j2)*cos(j4)*sin(j1)*sin(j3) + a4*cos(j3)*cos(j4)*sin(j1)*sin(j2) - a4*sin(j1)*sin(j2)*sin(j3)*sin(j4) 35 | cos(j2 + j3 + j4),- sin(j2 + j3 + j4 + j5)/2 - sin(j2 + j3 + j4 - j5)/2,cos(j2 + j3 + j4 - j5)/2 - cos(j2 + j3 + j4 + j5)/2,d1 + a3*cos(j2 + j3) + a2*cos(j2) + a4*cos(j2 + j3 + j4) 36 | 0,0,0,1 37 | 0T8 38 | sin(j6)*((cos(j1 + j2 + j3 + j4)*cos(j5))/2 - sin(j1)*sin(j5) + (cos(j2 - j1 + j3 + j4)*cos(j5))/2) + cos(j6)*(sin(j1 + j2 + j3 + j4)/2 + sin(j2 - j1 + j3 + j4)/2),cos(j6)*((cos(j1 + j2 + j3 + j4)*cos(j5))/2 - sin(j1)*sin(j5) + (cos(j2 - j1 + j3 + j4)*cos(j5))/2) - sin(j6)*(sin(j1 + j2 + j3 + j4)/2 + sin(j2 - j1 + j3 + j4)/2),- cos(j5)*sin(j1) - sin(j5)*(cos(j1 + j2 + j3 + j4)/2 + cos(j2 - j1 + j3 + j4)/2),a2*cos(j1)*sin(j2) - d3*sin(j1) - d4*sin(j1) - d5*sin(j1) - d2*sin(j1) - d6*cos(j5)*sin(j1) + a3*cos(j1)*cos(j2)*sin(j3) + a3*cos(j1)*cos(j3)*sin(j2) + a4*cos(j1)*cos(j2)*cos(j3)*sin(j4) + a4*cos(j1)*cos(j2)*cos(j4)*sin(j3) + a4*cos(j1)*cos(j3)*cos(j4)*sin(j2) + a5*cos(j1)*cos(j2)*cos(j3)*sin(j4) + a5*cos(j1)*cos(j2)*cos(j4)*sin(j3) + a5*cos(j1)*cos(j3)*cos(j4)*sin(j2) - a4*cos(j1)*sin(j2)*sin(j3)*sin(j4) - a5*cos(j1)*sin(j2)*sin(j3)*sin(j4) - d6*cos(j1)*cos(j2)*cos(j3)*cos(j4)*sin(j5) + d6*cos(j1)*cos(j2)*sin(j3)*sin(j4)*sin(j5) + d6*cos(j1)*cos(j3)*sin(j2)*sin(j4)*sin(j5) + d6*cos(j1)*cos(j4)*sin(j2)*sin(j3)*sin(j5) 39 | sin(j6)*(cos(j1)*sin(j5) - (cos(j5)*sin(j2 - j1 + j3 + j4))/2 + (sin(j1 + j2 + j3 + j4)*cos(j5))/2) - (cos(j6)*(cos(j1 + j2 + j3 + j4) - cos(j2 - j1 + j3 + j4)))/2,cos(j6)*(cos(j1)*sin(j5) - (cos(j5)*sin(j2 - j1 + j3 + j4))/2 + (sin(j1 + j2 + j3 + j4)*cos(j5))/2) + sin(j6)*(cos(j1 + j2 + j3 + j4)/2 - cos(j2 - j1 + j3 + j4)/2),cos(j1)*cos(j5) - (sin(j5)*(sin(j1 + j2 + j3 + j4) - sin(j2 - j1 + j3 + j4)))/2,d2*cos(j1) + d3*cos(j1) + d4*cos(j1) + d5*cos(j1) + d6*cos(j1)*cos(j5) + a2*sin(j1)*sin(j2) + a3*cos(j2)*sin(j1)*sin(j3) + a3*cos(j3)*sin(j1)*sin(j2) + a4*cos(j2)*cos(j3)*sin(j1)*sin(j4) + a4*cos(j2)*cos(j4)*sin(j1)*sin(j3) + a4*cos(j3)*cos(j4)*sin(j1)*sin(j2) + a5*cos(j2)*cos(j3)*sin(j1)*sin(j4) + a5*cos(j2)*cos(j4)*sin(j1)*sin(j3) + a5*cos(j3)*cos(j4)*sin(j1)*sin(j2) - a4*sin(j1)*sin(j2)*sin(j3)*sin(j4) - a5*sin(j1)*sin(j2)*sin(j3)*sin(j4) + d6*cos(j2)*sin(j1)*sin(j3)*sin(j4)*sin(j5) + d6*cos(j3)*sin(j1)*sin(j2)*sin(j4)*sin(j5) + d6*cos(j4)*sin(j1)*sin(j2)*sin(j3)*sin(j5) - d6*cos(j2)*cos(j3)*cos(j4)*sin(j1)*sin(j5) 40 | cos(j2 + j3 + j4 + j6)/2 + cos(j2 + j3 + j4 - j6)/2 - sin(j2 + j3 + j4)*cos(j5)*sin(j6),sin(j2 + j3 + j4 - j6)/2 - sin(j2 + j3 + j4 + j6)/2 - sin(j2 + j3 + j4)*cos(j5)*cos(j6),cos(j2 + j3 + j4 - j5)/2 - cos(j2 + j3 + j4 + j5)/2,d1 - (d6*cos(j2 + j3 + j4 + j5))/2 + a3*cos(j2 + j3) + (d6*cos(j2 + j3 + j4 - j5))/2 + a2*cos(j2) + a4*cos(j2 + j3 + j4) + a5*cos(j2 + j3 + j4) 41 | 0,0,0,1 42 | 0T9 43 | sin(j6)*((cos(j1 + j2 + j3 + j4)*cos(j5))/2 - sin(j1)*sin(j5) + (cos(j2 - j1 + j3 + j4)*cos(j5))/2) + cos(j6)*(sin(j1 + j2 + j3 + j4)/2 + sin(j2 - j1 + j3 + j4)/2),cos(j6)*((cos(j1 + j2 + j3 + j4)*cos(j5))/2 - sin(j1)*sin(j5) + (cos(j2 - j1 + j3 + j4)*cos(j5))/2) - sin(j6)*(sin(j1 + j2 + j3 + j4)/2 + sin(j2 - j1 + j3 + j4)/2),- cos(j5)*sin(j1) - sin(j5)*(cos(j1 + j2 + j3 + j4)/2 + cos(j2 - j1 + j3 + j4)/2),a2*cos(j1)*sin(j2) - d3*sin(j1) - d4*sin(j1) - d5*sin(j1) - d2*sin(j1) - d6*cos(j5)*sin(j1) - d7*cos(j5)*sin(j1) + a3*cos(j1)*cos(j2)*sin(j3) + a3*cos(j1)*cos(j3)*sin(j2) + a4*cos(j1)*cos(j2)*cos(j3)*sin(j4) + a4*cos(j1)*cos(j2)*cos(j4)*sin(j3) + a4*cos(j1)*cos(j3)*cos(j4)*sin(j2) + a5*cos(j1)*cos(j2)*cos(j3)*sin(j4) + a5*cos(j1)*cos(j2)*cos(j4)*sin(j3) + a5*cos(j1)*cos(j3)*cos(j4)*sin(j2) - a4*cos(j1)*sin(j2)*sin(j3)*sin(j4) - a5*cos(j1)*sin(j2)*sin(j3)*sin(j4) - d6*cos(j1)*cos(j2)*cos(j3)*cos(j4)*sin(j5) - d7*cos(j1)*cos(j2)*cos(j3)*cos(j4)*sin(j5) + d6*cos(j1)*cos(j2)*sin(j3)*sin(j4)*sin(j5) + d6*cos(j1)*cos(j3)*sin(j2)*sin(j4)*sin(j5) + d6*cos(j1)*cos(j4)*sin(j2)*sin(j3)*sin(j5) + d7*cos(j1)*cos(j2)*sin(j3)*sin(j4)*sin(j5) + d7*cos(j1)*cos(j3)*sin(j2)*sin(j4)*sin(j5) + d7*cos(j1)*cos(j4)*sin(j2)*sin(j3)*sin(j5) 44 | sin(j6)*(cos(j1)*sin(j5) - (cos(j5)*sin(j2 - j1 + j3 + j4))/2 + (sin(j1 + j2 + j3 + j4)*cos(j5))/2) - (cos(j6)*(cos(j1 + j2 + j3 + j4) - cos(j2 - j1 + j3 + j4)))/2,cos(j6)*(cos(j1)*sin(j5) - (cos(j5)*sin(j2 - j1 + j3 + j4))/2 + (sin(j1 + j2 + j3 + j4)*cos(j5))/2) + sin(j6)*(cos(j1 + j2 + j3 + j4)/2 - cos(j2 - j1 + j3 + j4)/2),cos(j1)*cos(j5) - (sin(j5)*(sin(j1 + j2 + j3 + j4) - sin(j2 - j1 + j3 + j4)))/2,d2*cos(j1) + d3*cos(j1) + d4*cos(j1) + d5*cos(j1) + d6*cos(j1)*cos(j5) + d7*cos(j1)*cos(j5) + a2*sin(j1)*sin(j2) + a3*cos(j2)*sin(j1)*sin(j3) + a3*cos(j3)*sin(j1)*sin(j2) + a4*cos(j2)*cos(j3)*sin(j1)*sin(j4) + a4*cos(j2)*cos(j4)*sin(j1)*sin(j3) + a4*cos(j3)*cos(j4)*sin(j1)*sin(j2) + a5*cos(j2)*cos(j3)*sin(j1)*sin(j4) + a5*cos(j2)*cos(j4)*sin(j1)*sin(j3) + a5*cos(j3)*cos(j4)*sin(j1)*sin(j2) - a4*sin(j1)*sin(j2)*sin(j3)*sin(j4) - a5*sin(j1)*sin(j2)*sin(j3)*sin(j4) + d6*cos(j2)*sin(j1)*sin(j3)*sin(j4)*sin(j5) + d6*cos(j3)*sin(j1)*sin(j2)*sin(j4)*sin(j5) + d6*cos(j4)*sin(j1)*sin(j2)*sin(j3)*sin(j5) + d7*cos(j2)*sin(j1)*sin(j3)*sin(j4)*sin(j5) + d7*cos(j3)*sin(j1)*sin(j2)*sin(j4)*sin(j5) + d7*cos(j4)*sin(j1)*sin(j2)*sin(j3)*sin(j5) - d6*cos(j2)*cos(j3)*cos(j4)*sin(j1)*sin(j5) - d7*cos(j2)*cos(j3)*cos(j4)*sin(j1)*sin(j5) 45 | cos(j2 + j3 + j4 + j6)/2 + cos(j2 + j3 + j4 - j6)/2 - sin(j2 + j3 + j4)*cos(j5)*sin(j6),sin(j2 + j3 + j4 - j6)/2 - sin(j2 + j3 + j4 + j6)/2 - sin(j2 + j3 + j4)*cos(j5)*cos(j6),cos(j2 + j3 + j4 - j5)/2 - cos(j2 + j3 + j4 + j5)/2,d1 - (d6*cos(j2 + j3 + j4 + j5))/2 - (d7*cos(j2 + j3 + j4 + j5))/2 + a3*cos(j2 + j3) + (d6*cos(j2 + j3 + j4 - j5))/2 + (d7*cos(j2 + j3 + j4 - j5))/2 + a2*cos(j2) + a4*cos(j2 + j3 + j4) + a5*cos(j2 + j3 + j4) 46 | 0,0,0,1 47 | -------------------------------------------------------------------------------- /MATLAB files/MatrixOutput/iMfile.txt: -------------------------------------------------------------------------------- 1 | Individual transformation matrices 2 | 0T1 3 | cos(j1),-sin(j1),0,0 4 | sin(j1),cos(j1),0,0 5 | 0,0,1,d1 6 | 0,0,0,1 7 | 1T2 8 | sin(j2),cos(j2),0,0 9 | 0,0,1,d2 10 | cos(j2),-sin(j2),0,0 11 | 0,0,0,1 12 | 2T3 13 | cos(j3),-sin(j3),0,a2 14 | sin(j3),cos(j3),0,0 15 | 0,0,1,d3 16 | 0,0,0,1 17 | 3T4 18 | cos(j4),-sin(j4),0,a3 19 | sin(j4),cos(j4),0,0 20 | 0,0,1,d4 21 | 0,0,0,1 22 | 4T5 23 | 0,-1,0,a4 24 | 1,0,0,0 25 | 0,0,1,d5 26 | 0,0,0,1 27 | 5T6 28 | cos(j5),-sin(j5),0,0 29 | 0,0,-1,0 30 | sin(j5),cos(j5),0,0 31 | 0,0,0,1 32 | 6T7 33 | 0,1,0,0 34 | 0,0,1,0 35 | 1,0,0,0 36 | 0,0,0,1 37 | 7T8 38 | cos(j6),-sin(j6),0,a5 39 | sin(j6),cos(j6),0,0 40 | 0,0,1,d6 41 | 0,0,0,1 42 | 8T9 43 | 1,0,0,0 44 | 0,1,0,0 45 | 0,0,1,d7 46 | 0,0,0,1 47 | -------------------------------------------------------------------------------- /MATLAB files/README.md: -------------------------------------------------------------------------------- 1 | # MATLAB code 2 | 3 | The MATLAB code is divided into three function files: *MDHMatrix.m*; *fwdKin.m*; and *invKin.m*, and a single script file: *main.m*. 4 | 5 | ## *MDHMatrix.m* Function File 6 | 7 | The *MDHMatrix.m* file contains a function capable of computing Modified Denavit-Hartenberg matrices. It receives as input a single line of a Denavit-Hartenberg matrix, *i.e.* a vector of 4 values [$\alpha$; $a$, $d$, $\theta$] that indicate the rotation around the x axis of frame i (in deg.), the translation along the x axis of frame i (in meters), the translation along the z axis of frame i (in meters), and the rotation around the z axis of frame i (in deg.), respectively, and outputs a 4x4 transformation matrix associated to that line of the Denavit-Hartenberg matrix. Summarily, it computes the transformation matrix from one system of coordinates to another. 8 | 9 | ```MATLAB 10 | %% Modified Denavit-Hartenberg Matrix 11 | 12 | % Transformation matrix from a system of coordinates to another. 13 | 14 | %% Function: MDHMatrix 15 | % 16 | % Computes the transformation matrix from a system of coordinates to 17 | % another. 18 | % 19 | % In: DHparams - Denavit-Hartenberg parameters matrix 20 | % |_ alpha - rotation around the x axis of frame i (deg) 21 | % |_ a - translation along the x axis of frame i (meters) 22 | % |_ d - translation along the z axis of frame i (meters) 23 | % |_ theta - rotation around the z axis of frame i (deg) 24 | % 25 | % Out: 4x4 transformation matrix from frame i-1 to i 26 | %% 27 | 28 | % Use this function to test the program regularly 29 | function T = MDHMatrix(DHparams) 30 | T = [ cosd(DHparams(4)) -sind(DHparams(4)) 0 DHparams(2) ; 31 | sind(DHparams(4))*cosd(DHparams(1)) cosd(DHparams(4))*cosd(DHparams(1)) -sind(DHparams(1)) -sind(DHparams(1))*DHparams(3); 32 | sind(DHparams(4))*sind(DHparams(1)) cosd(DHparams(4))*sind(DHparams(1)) cosd(DHparams(1)) cosd(DHparams(1))*DHparams(3) ; 33 | 0 0 0 1 ]; 34 | end 35 | 36 | % Use this function to print matrices 37 | % function T = MDHMatrix(DHparams) 38 | % T = [ cos(DHparams(4)) -sin(DHparams(4)) 0 DHparams(2) ; 39 | % sin(DHparams(4))*cos(DHparams(1)) cos(DHparams(4))*cos(DHparams(1)) -sin(DHparams(1)) -sin(DHparams(1))*DHparams(3); 40 | % sin(DHparams(4))*sin(DHparams(1)) cos(DHparams(4))*sin(DHparams(1)) cos(DHparams(1)) cos(DHparams(1))*DHparams(3) ; 41 | % 0 0 0 1 ]; 42 | % end 43 | ```` 44 | 45 | 46 | 47 | ## *fwdKin.m* Function File 48 | 49 | The *fwdKin.m* file is responsible for determining the general transformation matrix from the Denavit-Hartenberg parameters matrix. It returns a bi-dimensional array of arrays of matrices named *M*, where *M{1}* contains an array with the individual transformation matrices, and *M{2}* an array with the compound transformation matrices. To compute the individual transformation matrices the *MDHMatrix* function is used. 50 | 51 | ```MATLAB 52 | %% Forward kinematics 53 | % 54 | % 1st - Assign the reference frames along the robot's structure 55 | % 2nd - Define the Denavit-Hartenberg parameters 56 | % 3rd - Determine the individual transformation matrices 57 | % 4th - Determine the general transformation matrix to obtain the Cartesian 58 | % coordinates of the robot's tip in terms of its joint values. 59 | % 60 | 61 | %% Function: fwdKin 62 | % 63 | % Computes the position and orientation of a robot's tip. 64 | % 65 | % In: numFrames - number of reference frames (int) 66 | % DHMatrix - modified DH matrix (matrix) 67 | % 68 | % Out: M - two dimensional array of arrays of matrices 69 | % |_ M{1} - iT - array of individual transformation matrices 70 | % |_ M{2} - T0 - array of compound transformation matrices 71 | %% 72 | 73 | function M = fwdKin(DHMatrix) 74 | 75 | numFrames=size(DHMatrix); 76 | numFrames=numFrames(1); 77 | M=cell(1,2); 78 | %% Determine the individual transformation matrices 79 | 80 | %Create a cell array of individual transformations matrices i-1Ti according to 81 | %the number of reference frames 82 | iT = cell(1, numFrames); 83 | for i = 1 : numFrames 84 | iT{i} = zeros([4 4]); 85 | end 86 | 87 | %Compute the matrices 88 | for i = 1:numFrames 89 | iT{i} = MDHMatrix(DHMatrix(i,:)); 90 | end 91 | 92 | M{1}=iT; 93 | 94 | %% Determine the general transformation matrix 95 | 96 | %Create a cell array of compound transformations matrices 0Ti according to 97 | %the number of reference frames 98 | T0 = cell(1, numFrames); 99 | for i = 1 : numFrames-1 100 | T0{i} = zeros([4 4]); 101 | end 102 | 103 | T0{1}=iT{1}; 104 | 105 | %Compute the matrix 106 | for i = 2:numFrames 107 | T0{i} = T0{i-1}*iT{i}; 108 | end 109 | 110 | M{2}=T0; 111 | end 112 | ``` 113 | 114 | 115 | ## *invKin.m* Function File 116 | 117 | The *invKin.m* file computes the eight inverse kinematic solutions for the robot's joint angles using the equations from the geometric values of the robot's dimensions and the end-effector position. As input it receives two arrays with the robots link dimensions and the position and orientation of the end-effector, as output it returns a 8x6 matrix with the eight inverse solutions for the robot's six joints. 118 | 119 | ```MATLAB 120 | %% Inverse kinematics 121 | % 122 | % Using the ee position and orientation, determine the joint values. 123 | % 124 | %% 125 | 126 | %% Function: invKin 127 | % 128 | % Computes the joint values of the UR robots from their mechanical 129 | % dimensions and end-effector position 130 | % 131 | % In: d - array with robots dimensions 132 | % a - array with robots dimensions 133 | % eePosOri - 4x4 matrix with the position and orientation of the ee 134 | % 135 | % Out: joint - 8x6 matrix with the 8 ik solutions for the 6 joints 136 | %% 137 | 138 | function joint=invKin8sol(d, a, eePosOri) 139 | % 8 inv kin solutions, 6 joints 140 | ikSol=8; 141 | numJoints=6; 142 | joint=zeros(ikSol,numJoints); 143 | 144 | %% Computing theta1 145 | 146 | % 0P5 position of reference frame {5} in relation to {0} 147 | P=eePosOri*[0 0 -d(6)-d(7) 1].'; 148 | 149 | psi=atan2(P(2,1),P(1,1)); 150 | 151 | % There are two possible solutions for theta1, that depend on whether 152 | % the shoulder joint (joint 2) is left or right 153 | phi=acos( (d(2)+d(3)+d(4)+d(5)) / sqrt( P(2,1)^2 + P(1,1)^2 )); 154 | 155 | for i = 1:8 156 | joint(i,1)=(pi/2+psi-phi)-pi; 157 | end 158 | for i = 1:4 159 | joint(i,1)=(pi/2+psi+phi)-pi; 160 | end 161 | 162 | % From the eePosOri it is possible to know the tipPosOri 163 | T_67=MDHMatrix([0 0 d(7) 0]); 164 | %T_06=T_07*T_76 165 | T_06=eePosOri*inv(T_67); 166 | 167 | for j = 1:ikSol 168 | %% Computing theta5 169 | 170 | % Knowing theta1 it is possible to know 0T1 171 | T_01=MDHMatrix([0 0 d(1) rad2deg(joint(j,1))]); 172 | 173 | % 1T6 = 1T0 * 0T6 174 | T_16 = inv(T_01)*T_06; 175 | %1P6 176 | P_16=T_16(:,4); 177 | 178 | %Wrist up or down 179 | if(ismember(j,[1,2,5,6])) 180 | joint(j,5)=((acos((P_16(2,1)-(d(2)+d(3)+d(4)+d(5)))/d(6)))); 181 | else 182 | joint(j,5)=(-(acos((P_16(2,1)-(d(2)+d(3)+d(4)+d(5)))/d(6)))); 183 | end 184 | 185 | % Fix Error using atan2 / Inputs must be real. 186 | joint=real(double(rad2deg(joint))); 187 | joint=deg2rad(joint); 188 | 189 | %% Computing theta 6 190 | 191 | T_61=inv(T_16); 192 | % y1 seen from frame 6 193 | Y_16=T_61(:,2); 194 | 195 | % If theta 5 is equal to zero give arbitrary value to theta 6 196 | if(int8(rad2deg(real(joint(j,5)))) == 0 || int8(rad2deg(real(joint(j,5)))) == 2*pi) 197 | joint(j,6)=deg2rad(0); 198 | else 199 | joint(j,6)= (pi/2 + atan2( -Y_16(2,1)/sin(joint(j,5)) , Y_16(1,1)/sin(joint(j,5)))); 200 | end 201 | 202 | %% Computing theta 3, 2 and 4 203 | 204 | %Get position of frame 4 from frame 1, P_14 205 | 206 | %The value of T_01, T_45, and T_56 are known since joints{1,5,6} have 207 | %been obtained 208 | 209 | %T_45 = T_44'*T_4'5 210 | T_44_=MDHMatrix([0 a(4) d(5) 90]); 211 | T_4_5=MDHMatrix([90 0 0 rad2deg(joint(j,5))]); 212 | T_45 = T_44_*T_4_5; 213 | 214 | %T_56 = T_55'*T_5'6 215 | T_55_=MDHMatrix([-90 0 0 -90]); 216 | T_5_6=MDHMatrix([0 a(5) d(6) rad2deg(joint(j,6))]); 217 | T_56 = T_55_*T_5_6; 218 | 219 | T_46 = T_45*T_56; 220 | 221 | T_64 = inv(T_46); 222 | 223 | T_14=T_16*T_64; 224 | 225 | %P_14 is the fourth column of T_14 226 | P_14=T_14(:,4); 227 | 228 | P_14_xz=sqrt( P_14(1)^2 + P_14(3)^2); 229 | 230 | %Elbow up or down 231 | if(rem(j,2)==0) 232 | psi= acos( ( P_14_xz^2-a(3)^2-a(2)^2 )/( -2*a(2)*a(3) ) ); 233 | joint(j,3)=( pi - psi); 234 | % Masking theta3 for CoppeliaSim (invert value for ang>180) 235 | if(joint(j,3)>pi) 236 | joint(j,3)=joint(j,3)-pi*2; 237 | end 238 | % theta 2 239 | joint(j,2) =pi/2 - (atan2( P_14(3), +P_14(1)) + asin( (a(3)*sin(psi))/P_14_xz)); 240 | % theta 4 241 | % Fix Error using atan2 / Inputs must be real. 242 | joint=real(double(rad2deg(joint))); 243 | joint=deg2rad(joint); 244 | joint(j,4) = joint4(d, a, joint(j,2), joint(j,3), T_06, T_01, T_64); 245 | else 246 | psi= acos( ( P_14_xz^2-a(3)^2-a(2)^2 )/( -2*a(2)*a(3) ) ); 247 | joint(j,3)=( pi + psi); 248 | % Masking theta3 for CoppeliaSim (invert value for ang>180) 249 | if(joint(j,3)>pi) 250 | joint(j,3)=joint(j,3)-pi*2; 251 | end 252 | % theta 2 253 | joint(j,2) = pi/2 - (atan2( P_14(3), +P_14(1)) + asin( (a(3)*sin(-psi))/P_14_xz)); 254 | % theta 4 255 | % Fix Error using atan2 / Inputs must be real. 256 | joint=real(double(rad2deg(joint))); 257 | joint=deg2rad(joint); 258 | joint(j,4) = joint4(d, a, joint(j,2), joint(j,3), T_06, T_01, T_64); 259 | end 260 | end 261 | % Fix Error using atan2 / Inputs must be real. 262 | joint=real(double(rad2deg(joint))); 263 | joint=deg2rad(joint); 264 | end 265 | 266 | %% Computing theta4 267 | function joint4=joint4(d, a, theta2, theta3, T_06, T_01, T_64) 268 | 269 | T_12=MDHMatrix([-90 0 d(2) rad2deg(theta2)-90]); 270 | T_23=MDHMatrix([0 a(2) d(3) rad2deg(theta3)]); 271 | T_03=T_01*T_12*T_23; 272 | 273 | T_36=inv(T_03)*T_06; 274 | T_34=T_36*T_64; 275 | 276 | x_34=T_34(:,1); 277 | 278 | joint4 = (atan2(x_34(2),x_34(1))); 279 | end 280 | ``` 281 | 282 | 283 | ## *main.m* Script File 284 | 285 | 286 | The *main.m* script is where the *fwdKin* and *invKin* functions are called. This script can be broken down into three sections: 287 | 288 | - **Initialisation**: where variables are initialised and the connection with *CoppeliaSim* is set up; 289 | - **User Interface**: where the user specifies the values of the dimensions of the robot, and the target joint values. It is also here that the Denavit-Hartenberg matrix is coded; 290 | - **Main Program**: section responsible for checking the connection with *CoppeliaSim*, retrieving the joint handles from the robot model in *CoppeliaSim*, computing the forward and inverse kinematics for the target joint values, and sending the output from the inverse kinematic solution to the robot model in *CoppeliaSim*. 291 | 292 | This program is intended to test the correct computation of the end-effector position from a set of target joint values, and vice-versa, compared to the physical representation of the robot in *CoppeliaSim*. 293 | 294 | ```MATLAB 295 | %% Initialization 296 | 297 | disp('Program started'); 298 | 299 | dof=6; % degrees of freedom UR10 300 | d=zeros(1,dof+1); %distances 301 | a=zeros(1,dof); %distances 302 | theta=zeros(1,dof); %joint angles 303 | jh=zeros(1,dof); %CoppeliaSim joint handles 304 | totalIKsol=8; %number of inverse kinematic solutions 305 | printTimeInterval=0.006; %correct time interval for printing variables in CoppeliaSim 306 | 307 | % Do the connection with CoppeliaSim 308 | 309 | % sim=remApi('remoteApi','extApi.h'); % using the header (requires a compiler) 310 | sim=remApi('remoteApi'); % using the prototype file (remoteApiProto.m) 311 | sim.simxFinish(-1); % just in case, close all opened connections 312 | clientID=sim.simxStart('127.0.0.1',19999,true,true,5000,5); 313 | 314 | %% Denavit-Hartenberg parameters (User interface) 315 | 316 | % Distances from the mechanical drawing of the UR10 317 | % These values can be changed to accomodate any of the UR series robots 318 | % d(1)=0.128; 319 | % d(2)=0.088; 320 | % d(3)=0.024; 321 | % d(4)=-0.006; 322 | % d(5)=0.058; 323 | % d(6)=0.092; 324 | % d(7)=0.10; 325 | % 326 | % a(2)=0.612; 327 | % a(3)=0.572; 328 | % a(4)=0.058; 329 | % a(5)=0.058; 330 | 331 | % CoppeliaSim link dimensions for the UR10 model 332 | d(1)=0.109; 333 | d(2)=0.101222; 334 | d(3)=0.01945; 335 | d(4)=-0.006; 336 | d(5)=0.0585; 337 | d(6)=0.0572+0.03434;%to the tip 338 | d(7)=0.10185; 339 | 340 | a(2)=0.612; 341 | a(3)=0.573; 342 | a(4)=0.0567; 343 | a(5)=0.059; 344 | 345 | % Target joint angles 346 | % Select here the target joint angle you want the robot to assume 347 | theta(1)=35; 348 | theta(2)=0; 349 | theta(3)=90; 350 | theta(4)=50; 351 | theta(5)=30; 352 | theta(6)=20; 353 | 354 | %Definition of the modified Denavit-Hartenberg matrix (Do not change!) 355 | DHMatrix = [ 0 0 d(1) theta(1); % 1 0T1 356 | -90 0 d(2) theta(2)-90; % 2 1T2 357 | 0 a(2) d(3) theta(3); % 3 2T3 358 | 0 a(3) d(4) theta(4); % 4 3T4 359 | 0 a(4) d(5) 90; % 4' 4T4' 5 360 | 90 0 0 theta(5); % 5 4'T5 6 361 | -90 0 0 -90; % 5' 5T5' 7 362 | 0 a(5) d(6) theta(6); % 6 5'T6 8 363 | 0 0 d(7) 0;]; % 7 6T7 9 364 | 365 | %% Main program 366 | 367 | % Determine the number of reference frames using the DHMatrix 368 | numFrames=size(DHMatrix); 369 | numFrames=numFrames(1); 370 | 371 | if (clientID>-1) 372 | disp('Connected to remote API server'); 373 | 374 | % Retreive joint handles from CoppeliaSim 375 | for i = 1 : dof 376 | [r,jh(i)]=sim.simxGetObjectHandle(clientID, strcat('UR10_joint', int2str(i)) , sim.simx_opmode_blocking); 377 | end 378 | disp('_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-') 379 | %% Compute forward kinematics 380 | M=fwdKin(DHMatrix); 381 | disp('Forward kinematics solution') 382 | % Print end-effector position 383 | disp('End effector position in meters:') 384 | disp(M{2}{numFrames}(:,4)); 385 | % Print robot's tip position 386 | disp('Robot´s tip position in meters:') 387 | disp(M{2}{numFrames-1}(:,4)); 388 | disp('_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-') 389 | %% Compute inverse kinematics 390 | joints=(invKin8sol(d,a,M{2}{numFrames}(:,:))); 391 | % Print the joint values for every IK solution 392 | disp('Inverse kinematics solutions:') 393 | disp(int32(rad2deg(joints(:,:)))); 394 | 395 | % Send joint values to CoppeliaSim 396 | for i = 1: totalIKsol 397 | disp('_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-') 398 | fprintf('Inverse kinematic solution %d\n',i); 399 | disp('Value in degrees'); 400 | disp(rad2deg(joints(i,:))); 401 | disp('Value in radians'); 402 | disp(joints(i,:)); 403 | pause(0.5); 404 | for j = 1 : dof 405 | sim.simxSetJointTargetPosition(clientID, jh(j), (joints(i,j)), sim.simx_opmode_streaming); 406 | end 407 | pause(0.5); 408 | sim.simxSetIntegerSignal(clientID, 'showPos', 1, sim.simx_opmode_streaming); 409 | pause(printTimeInterval); 410 | sim.simxSetIntegerSignal(clientID, 'showPos', 0, sim.simx_opmode_streaming); 411 | end 412 | 413 | else 414 | disp('Failed connecting to remote API server'); 415 | end 416 | sim.delete(); % call the destructor! 417 | 418 | disp('Program ended'); 419 | ``` 420 | -------------------------------------------------------------------------------- /MATLAB files/RPY.m: -------------------------------------------------------------------------------- 1 | %% Function: RPY 2 | % 3 | % Computes the Euler Angles RPY 4 | % 5 | % In: iTx - an individual transformation matrix 6 | % 7 | % Out: RPY - [alpha, beta, gamma] 8 | %% 9 | function RPY = RPY(R) 10 | 11 | if R(1,3) == 1 || R(1,3) == -1 12 | %special case 13 | gamma = 0; %set arbitrarily 14 | dlta = atan2(R(1,2),R(1,3)); 15 | if R(1,3) == -1 16 | beta = pi/2; 17 | alpha = gamma + dlta; 18 | else 19 | beta = -pi/2; 20 | alpha = -gamma + dlta; 21 | end 22 | else 23 | beta = - asin(R(1,3)); 24 | alpha = atan2(R(2,3)/cos(beta), R(3,3)/cos(beta)); 25 | gamma = atan2(R(1,2)/cos(beta), R(1,1)/cos(beta)); 26 | end 27 | 28 | RPY = rad2deg([-alpha -beta -gamma]); 29 | end 30 | -------------------------------------------------------------------------------- /MATLAB files/ValidationOutput/stdev.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/MATLAB files/ValidationOutput/stdev.xlsx -------------------------------------------------------------------------------- /MATLAB files/fwdKin.m: -------------------------------------------------------------------------------- 1 | %% Forward kinematics 2 | % 3 | % 1st - Assign the reference frames along the robot's structure 4 | % 2nd - Define the Denavit-Hartenberg parameters 5 | % 3rd - Determine the individual transformation matrices 6 | % 4th - Determine the general transformation matrix to obtain the Cartesian 7 | % coordinates of the robot's tip in terms of its joint values. 8 | % 9 | 10 | %% Function: fwdKin 11 | % 12 | % Computes the position and orientation of a robot's tip. 13 | % 14 | % In: numFrames - number of reference frames (int) 15 | % DHMatrix - modified DH matrix (matrix) 16 | % 17 | % Out: M - two dimensional array of arrays of matrices 18 | % |_ M{1} - iT - array of individual transformation matrices 19 | % |_ M{2} - T0 - array of compound transformation matrices 20 | %% 21 | 22 | function M = fwdKin(DHMatrix) 23 | 24 | numFrames=size(DHMatrix); 25 | numFrames=numFrames(1); 26 | M=cell(1,2); 27 | %% Determine the individual transformation matrices 28 | 29 | %Create a cell array of individual transformations matrices i-1Ti according to 30 | %the number of reference frames 31 | iT = cell(1, numFrames); 32 | for i = 1 : numFrames 33 | iT{i} = zeros([4 4]); 34 | end 35 | 36 | %Compute the matrices 37 | for i = 1:numFrames 38 | iT{i} = MDHMatrix(DHMatrix(i,:)); 39 | end 40 | 41 | M{1}=iT; 42 | 43 | %% Determine the general transformation matrix 44 | 45 | %Create a cell array of compound transformations matrices 0Ti according to 46 | %the number of reference frames 47 | T0 = cell(1, numFrames); 48 | for i = 1 : numFrames-1 49 | T0{i} = zeros([4 4]); 50 | end 51 | 52 | T0{1}=iT{1}; 53 | 54 | %Compute the matrix 55 | for i = 2:numFrames 56 | T0{i} = T0{i-1}*iT{i}; 57 | end 58 | 59 | M{2}=T0; 60 | end -------------------------------------------------------------------------------- /MATLAB files/invKin8sol.m: -------------------------------------------------------------------------------- 1 | %% Inverse kinematics 2 | % 3 | % Using the ee position and orientation, determine the joint values. 4 | % 5 | %% 6 | 7 | %% Function: invKin 8 | % 9 | % Computes the joint values of the UR robots from their mechanical 10 | % dimensions and end-effector position 11 | % 12 | % In: d - array with robots dimensions 13 | % a - array with robots dimensions 14 | % eePosOri - 4x4 matrix with the position and orientation of the ee 15 | % 16 | % Out: joint - 8x6 matrix with the 8 ik solutions for the 6 joints 17 | %% 18 | 19 | function joint=invKin8sol(d, a, eePosOri) 20 | % 8 inv kin solutions, 6 joints 21 | ikSol=8; 22 | numJoints=6; 23 | joint=zeros(ikSol,numJoints); 24 | 25 | %% Computing theta1 26 | 27 | % 0P5 position of reference frame {5} in relation to {0} 28 | P=eePosOri*[0 0 -d(6)-d(7) 1].'; 29 | 30 | psi=atan2(P(2,1),P(1,1)); 31 | 32 | % There are two possible solutions for theta1, that depend on whether 33 | % the shoulder joint (joint 2) is left or right 34 | phi=acos( (d(2)+d(3)+d(4)+d(5)) / sqrt( P(2,1)^2 + P(1,1)^2 )); 35 | 36 | for i = 1:8 37 | joint(i,1)=(pi/2+psi-phi)-pi; 38 | end 39 | for i = 1:4 40 | joint(i,1)=(pi/2+psi+phi)-pi; 41 | end 42 | 43 | % From the eePosOri it is possible to know the tipPosOri 44 | T_67=MDHMatrix([0 0 d(7) 0]); 45 | T_06=eePosOri/T_67; 46 | 47 | for j = 1:ikSol 48 | %% Computing theta5 49 | 50 | % Knowing theta1 it is possible to know 0T1 51 | T_01=MDHMatrix([0 0 d(1) rad2deg(joint(j,1))]); 52 | 53 | % 1T6 = 1T0 * 0T6 54 | T_16 = T_01\T_06; 55 | %1P6 56 | P_16=T_16(:,4); 57 | 58 | %Wrist up or down 59 | if(ismember(j,[1,2,5,6])) 60 | joint(j,5)=( (acos( (P_16(2,1)-(d(2)+d(3)+d(4)+d(5) ) ) / d(6)) ) ); 61 | else 62 | joint(j,5)=(-(acos((P_16(2,1)-(d(2)+d(3)+d(4)+d(5)))/d(6)))); 63 | end 64 | 65 | %% Computing theta 6 66 | 67 | T_61=inv(T_16); 68 | % y1 seen from frame 6 69 | Y_16=T_61(:,2); 70 | % Fix Error using atan2 / Inputs must be real. 71 | joint=real(double(rad2deg(joint))); 72 | joint=deg2rad(joint); 73 | % If theta 5 is equal to zero give arbitrary value to theta 6 74 | if(int8(rad2deg(real(joint(j,5)))) == 0 || int8(rad2deg(real(joint(j,5)))) == 2*pi) 75 | joint(j,6) = deg2rad(0); 76 | else 77 | joint(j,6) = (pi/2 + atan2( -Y_16(2,1)/sin(joint(j,5)) , Y_16(1,1)/sin(joint(j,5)))); 78 | %joint(j,6) = (atan2( Y_16(2,1)/sin(joint(j,5)) , Y_16(1,1)/sin(joint(j,5)))); 79 | end 80 | 81 | %% Computing theta 3, 2 and 4 82 | 83 | %Get position of frame 4 from frame 1, P_14 84 | 85 | %The value of T_01, T_45, and T_56 are known since joints{1,5,6} have 86 | %been obtained 87 | 88 | %T_45 = T_44'*T_4'5 89 | T_44_=MDHMatrix([0 a(4) d(5) 90]); 90 | T_4_5=MDHMatrix([90 0 0 rad2deg(joint(j,5))]); 91 | T_45 = T_44_*T_4_5; 92 | 93 | %T_56 = T_55'*T_5'6 94 | T_55_=MDHMatrix([-90 0 0 -90]); 95 | T_5_6=MDHMatrix([0 a(5) d(6) rad2deg(joint(j,6))]); 96 | T_56 = T_55_*T_5_6; 97 | 98 | T_46 = T_45*T_56; 99 | 100 | T_64 = inv(T_46); 101 | 102 | T_14=T_16*T_64; 103 | 104 | %P_14 is the fourth column of T_14 105 | P_14=T_14(:,4); 106 | 107 | P_14_xz=sqrt( P_14(1)^2 + P_14(3)^2); 108 | 109 | %Elbow up or down 110 | if(rem(j,2)==0) 111 | psi= acos( ( P_14_xz^2-a(3)^2-a(2)^2 )/( -2*a(2)*a(3) ) ); 112 | joint(j,3)=( pi - psi); 113 | % Masking theta3 for CoppeliaSim (invert value for ang>180) 114 | if(joint(j,3)>pi) 115 | joint(j,3)=joint(j,3)-pi*2; 116 | end 117 | % theta 2 118 | % Fix Error using atan2 / Inputs must be real. 119 | joint=real(double(rad2deg(joint))); 120 | joint=deg2rad(joint); 121 | joint(j,2) =pi/2 - (atan2( P_14(3), +P_14(1)) + asin( (a(3)*sin(psi))/P_14_xz)); 122 | % theta 4 123 | % Fix Error using atan2 / Inputs must be real. 124 | joint=real(double(rad2deg(joint))); 125 | joint=deg2rad(joint); 126 | joint(j,4) = joint4(d, a, joint(j,2), joint(j,3), T_06, T_01, T_64); 127 | else 128 | psi= acos( ( P_14_xz^2-a(3)^2-a(2)^2 )/( -2*a(2)*a(3) ) ); 129 | joint(j,3)=( pi + psi); 130 | % Masking theta3 for CoppeliaSim (invert value for ang>180) 131 | if(joint(j,3)>pi) 132 | joint(j,3)=joint(j,3)-pi*2; 133 | end 134 | % theta 2 135 | % Fix Error using atan2 / Inputs must be real. 136 | joint=real(double(rad2deg(joint))); 137 | joint=deg2rad(joint); 138 | joint(j,2) = pi/2 - (atan2( P_14(3), +P_14(1)) + asin( (a(3)*sin(-psi))/P_14_xz)); 139 | % theta 4 140 | % Fix Error using atan2 / Inputs must be real. 141 | joint=real(double(rad2deg(joint))); 142 | joint=deg2rad(joint); 143 | joint(j,4) = joint4(d, a, joint(j,2), joint(j,3), T_06, T_01, T_64); 144 | end 145 | end 146 | % Fix Error using atan2 / Inputs must be real. 147 | joint=real(double(rad2deg(joint))); 148 | joint=deg2rad(joint); 149 | end 150 | 151 | %% Computing theta4 152 | function joint4=joint4(d, a, theta2, theta3, T_06, T_01, T_64) 153 | 154 | T_12=MDHMatrix([-90 0 d(2) rad2deg(theta2)-90]); 155 | T_23=MDHMatrix([0 a(2) d(3) rad2deg(theta3)]); 156 | T_03=T_01*T_12*T_23; 157 | 158 | T_36=T_03\T_06; 159 | T_34=T_36*T_64; 160 | 161 | x_34=T_34(:,1); 162 | 163 | joint4 = (atan2(x_34(2),x_34(1))); 164 | end -------------------------------------------------------------------------------- /MATLAB files/printM.m: -------------------------------------------------------------------------------- 1 | %% Initialization 2 | 3 | disp('Program started'); 4 | 5 | syms j [1 6]; % joints 6 | syms d [1 7]; % link dimensions x 7 | syms a [1 5]; % link dimensions z 8 | 9 | dof=6; % degrees of freedom UR10 10 | 11 | iMfile='.\MatrixOutput\iMfile.txt'; % individual matrices output file 12 | cMfile='.\MatrixOutput\cMfile.txt'; % compound matrices output file 13 | 14 | aux_matrix=strings(4,4); %auxiliary matrix of strings to correctly output the matrices 15 | 16 | %Definition of the modified Denavit-Hartenberg matrix (Do not change!) 17 | DHMatrix = [ 0 0 d1 j1; % 1 0T1 18 | -pi/2 0 d2 j2-pi/2; % 2 1T2 19 | 0 a2 d3 j3; % 3 2T3 20 | 0 a3 d4 j4; % 4 3T4 21 | 0 a4 d5 pi/2; % 4' 4T4' 5 22 | pi/2 0 0 j5; % 5 4'T5 6 23 | -pi/2 0 0 -pi/2; % 5' 5T5' 7 24 | 0 a5 d6 j6; % 6 5'T6 8 25 | 0 0 d7 0;]; % 7 6T7 9 26 | 27 | %% Main program 28 | 29 | % Determine the number of reference frames using the DHMatrix 30 | numFrames=size(DHMatrix); 31 | numFrames=numFrames(1); 32 | 33 | % Compute forward kinematics 34 | M=fwdKin(DHMatrix); 35 | 36 | % Output matrices 37 | disp('Outputing forward kinematics solution to iMfile.txt and cMfile.txt'); 38 | 39 | writematrix("Individual transformation matrices",iMfile); 40 | for i = 1 : numFrames 41 | writematrix(strcat(num2str(i-1),'T',num2str(i)),iMfile,'WriteMode','append'); 42 | for j = 1 : 4 43 | for k = 1 : 4 44 | aux_matrix(j,k)=char(simplify(M{1}{i}(j,k))); 45 | end 46 | end 47 | writematrix(aux_matrix,iMfile,'WriteMode','append'); 48 | end 49 | 50 | writematrix("Compound transformation matrices",cMfile); 51 | for i = 1 : numFrames 52 | writematrix(strcat(num2str(0),'T',num2str(i)),cMfile,'WriteMode','append'); 53 | for j = 1 : 4 54 | for k = 1 : 4 55 | 56 | aux_matrix(j,k)=char(simplify(M{2}{i}(j,k))); 57 | end 58 | end 59 | writematrix(aux_matrix,cMfile,'WriteMode','append'); 60 | end 61 | 62 | disp('Program ended'); -------------------------------------------------------------------------------- /MATLAB files/remoteApi.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/MATLAB files/remoteApi.dll -------------------------------------------------------------------------------- /MATLAB files/runSim.m: -------------------------------------------------------------------------------- 1 | function runSim(robot, theta1, theta2, theta3, theta4, theta5, theta6) 2 | 3 | %% Initialization 4 | 5 | disp('Program started'); 6 | 7 | dof=6; % degrees of freedom UR10 8 | d=zeros(1,dof+1); %distances 9 | a=zeros(1,dof); %distances 10 | theta=zeros(1,dof); %joint angles 11 | jh=zeros(1,dof); %CoppeliaSim joint handles 12 | totalIKsol=8; %number of inverse kinematic solutions 13 | printTimeInterval=0.006; %correct time interval for printing variables in CoppeliaSim 14 | 15 | % Do the connection with CoppeliaSim 16 | 17 | % sim=remApi('remoteApi','extApi.h'); % using the header (requires a compiler) 18 | sim=remApi('remoteApi'); % using the prototype file (remoteApiProto.m) 19 | sim.simxFinish(-1); % just in case, close all opened connections 20 | clientID=sim.simxStart('127.0.0.1',19999,true,true,5000,5); 21 | 22 | %% Denavit-Hartenberg parameters (User interface) 23 | 24 | % CoppeliaSim link dimensions for the UR models (to get better results) 25 | switch robot 26 | case "UR10" 27 | d(1)=0.109; 28 | d(2)=0.10122; 29 | d(3)=0.12067-0.10122; 30 | d(4)=0.11406-0.12067; 31 | d(5)=0.17246-0.11406; 32 | d(6)=0.26612-0.17246; 33 | %d(7)=0.36474-0.26612; 34 | d(7)=0; 35 | 36 | a(2)=0.7211-0.109; 37 | a(3)=1.2933-0.7211; 38 | a(4)=1.3506-1.2933; 39 | a(5)=1.409-1.3506; 40 | joint_handle='UR10_joint'; 41 | case "UR5" 42 | d(1)=0.07455; 43 | d(2)=0.0703; 44 | d(3)=0.0703-0.0703; 45 | d(4)=0.0703-0.0703; 46 | d(5)=0.11-0.0703; 47 | d(6)=0.1929-0.11; 48 | %d(7)=0.36474-0.26612; 49 | d(7)=0; 50 | 51 | a(2)=0.49965-0.07455; 52 | a(3)=0.8918-0.49965; 53 | a(4)=0.93737-0.8918; 54 | a(5)=0.98655-0.93737; 55 | joint_handle='UR5_joint'; 56 | case "UR3" 57 | d(1)=0.10887; 58 | d(2)=0.11154; 59 | d(3)=0.11154-0.11154; 60 | d(4)=0.11154-0.11154; 61 | d(5)=0.11223-0.11154; 62 | d(6)=0.194-0.11223; 63 | %d(7)=0.36474-0.26612; 64 | d(7)=0; 65 | 66 | a(2)=0.35252-0.10887; 67 | a(3)=0.56577-0.35252; 68 | a(4)=0.64999-0.56577; 69 | a(5)=0.65111-0.64999; 70 | joint_handle='UR3_joint'; 71 | otherwise 72 | disp('You didn´t select a robot!'); 73 | end 74 | 75 | % Target joint angles 76 | theta(1)=(str2double(theta1)); 77 | theta(2)=(str2double(theta2)); 78 | theta(3)=(str2double(theta3)); 79 | theta(4)=(str2double(theta4)); 80 | theta(5)=(str2double(theta5)); 81 | theta(6)=(str2double(theta6)); 82 | 83 | %Definition of the modified Denavit-Hartenberg matrix (Do not change!) 84 | DHMatrix = [ 0 0 d(1) theta(1); % 1 0T1 85 | (-90) 0 d(2) theta(2)-(90); % 2 1T2 86 | 0 a(2) d(3) theta(3); % 3 2T3 87 | 0 a(3) d(4) theta(4); % 4 3T4 88 | 0 a(4) d(5) (90); % 4' 4T4' 5 89 | 90 0 0 theta(5); % 5 4'T5 6 90 | -(90) 0 0 -(90); % 5' 5T5' 7 91 | 0 a(5) d(6) theta(6); % 6 5'T6 8 92 | 0 0 d(7) 0;]; % 7 6T7 9 93 | 94 | %% Main program 95 | 96 | % Determine the number of reference frames using the DHMatrix 97 | numFrames=size(DHMatrix); 98 | numFrames=numFrames(1); 99 | 100 | if (clientID>-1) 101 | disp('Connected to remote API server'); 102 | 103 | % Retreive joint handles from CoppeliaSim 104 | for i = 1 : dof 105 | [~,jh(i)]=sim.simxGetObjectHandle(clientID, strcat(joint_handle, int2str(i)) , sim.simx_opmode_blocking); 106 | end 107 | disp('_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-') 108 | %% Compute forward kinematics 109 | M=fwdKin(DHMatrix); 110 | R=M{2}{numFrames-1}([1,2,3],[1,2,3]); 111 | disp('Forward kinematics solution') 112 | % Print end-effector position 113 | disp('End effector position in meters:') 114 | disp(M{2}{numFrames}(:,4).'); 115 | % Print robot's tip position 116 | disp('Robot´s tip position in meters:') 117 | fwd_tip_pose = M{2}{numFrames-1}(:,4).'; 118 | disp(fwd_tip_pose(1:3)); 119 | % Print robot's tip and end-effector orientation 120 | disp('Robot´s tip and end-effector orientation in degrees:') 121 | disp(RPY(R)); 122 | disp('_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-') 123 | %% Compute inverse kinematics 124 | joints=(invKin8sol(d,a,M{2}{numFrames}(:,:))); 125 | % Print the joint values for every IK solution 126 | disp('Inverse kinematics solutions:') 127 | disp(int32(rad2deg(joints(:,:)))); 128 | 129 | % Send joint values to CoppeliaSim 130 | for i = 1: totalIKsol 131 | disp('_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-') 132 | fprintf('Inverse kinematic solution %d\n',i); 133 | disp('Value in degrees'); 134 | disp(rad2deg(joints(i,:))); 135 | disp('Value in radians'); 136 | disp(joints(i,:)); 137 | pause(0.5); 138 | for j = 1 : dof 139 | % Send joint values for robot model 140 | sim.simxSetJointTargetPosition(clientID, jh(j), (joints(i,j)), sim.simx_opmode_blocking); 141 | end 142 | pause(0.5); 143 | % Allow Coppelia to display tip pose 144 | sim.simxSetIntegerSignal(clientID, 'showPos', 1, sim.simx_opmode_blocking); 145 | pause(printTimeInterval); 146 | sim.simxSetIntegerSignal(clientID, 'showPos', 0, sim.simx_opmode_blocking); 147 | end 148 | else 149 | disp('Failed connecting to remote API server'); 150 | end 151 | 152 | sim.delete(); % call the destructor! 153 | 154 | disp('Program ended'); 155 | 156 | end 157 | 158 | -------------------------------------------------------------------------------- /MATLAB files/validationTest.m: -------------------------------------------------------------------------------- 1 | %% Initialization 2 | 3 | disp('Program started'); 4 | 5 | dof=6; % degrees of freedom UR10 6 | d=zeros(1,dof+1); %distances 7 | a=zeros(1,dof); %distances 8 | theta=zeros(1,dof); %joint angles 9 | jh=zeros(1,dof); %CoppeliaSim joint handles 10 | totalIKsol=8; %number of inverse kinematic solutions 11 | iterations=1000; %number of code iterations 12 | error=zeros(1,totalIKsol); %error of ik sols 13 | 14 | ValFile='.\ValidationOutput\stdev.xlsx'; % error and computation times output file 15 | 16 | %% Denavit-Hartenberg parameters (User interface) 17 | 18 | % CoppeliaSim link dimensions for the UR10 model 19 | d(1)=0.109; 20 | d(2)=0.10122; 21 | d(3)=0.12067-0.10122; 22 | d(4)=0.11406-0.12067; 23 | d(5)=0.17246-0.11406; 24 | d(6)=0.26612-0.17246; 25 | d(7)=0; % end-effector 26 | 27 | a(2)=0.7211-0.109; 28 | a(3)=1.2933-0.7211; 29 | a(4)=1.3506-1.2933; 30 | a(5)=1.409-1.3506; 31 | 32 | %% Main program 33 | 34 | for it = 1 : iterations 35 | 36 | % Set random joint values 37 | for j = 1 : dof 38 | theta(j)=randi([-360 360],1,1); 39 | end 40 | %Definition of the modified Denavit-Hartenberg matrix (Do not change!) 41 | DHMatrix = [ 0 0 d(1) theta(1); % 1 0T1 42 | -90 0 d(2) theta(2)-90; % 2 1T2 43 | 0 a(2) d(3) theta(3); % 3 2T3 44 | 0 a(3) d(4) theta(4); % 4 3T4 45 | 0 a(4) d(5) 90; % 4' 4T4' 5 46 | 90 0 0 theta(5); % 5 4'T5 6 47 | -90 0 0 -90; % 5' 5T5' 7 48 | 0 a(5) d(6) theta(6); % 6 5'T6 8 49 | 0 0 d(7) 0;]; % 7 6T7 9 50 | 51 | % Determine the number of reference frames using the DHMatrix 52 | numFrames=size(DHMatrix); 53 | numFrames=numFrames(1); 54 | disp('_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-') 55 | %% Compute forward kinematics 56 | % Time and execute the function 57 | f=@() fwdKin(DHMatrix); 58 | t(1)=timeit(f); 59 | M=fwdKin(DHMatrix); 60 | R=M{2}{numFrames-1}([1,2,3],[1,2,3]); 61 | disp('Forward kinematics solution') 62 | % Print robot's tip position 63 | disp('Robot´s tip position in meters:') 64 | fwd_tip_pos = M{2}{numFrames-1}(:,4).'; 65 | disp(fwd_tip_pos(1:3)); 66 | % Print robot's tip and end-effector orientation 67 | disp('Robot´s tip orientation in degrees:') 68 | fwd_tip_ori = RPY(R); 69 | disp(fwd_tip_ori); 70 | fwd_tip_pose = [fwd_tip_pos(1:3) fwd_tip_ori]; 71 | disp('_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-') 72 | %% Compute inverse kinematics 73 | % Time and execute the function 74 | f=@() invKin8sol(d,a,M{2}{numFrames}(:,:)); 75 | t(2)=timeit(f); 76 | joints=(invKin8sol(d,a,M{2}{numFrames}(:,:))); 77 | % Print the joint values for every IK solution 78 | disp('Inverse kinematics solutions:') 79 | disp(int32(rad2deg(joints(:,:)))); 80 | % Print and validate IK solutions 81 | for i = 1: totalIKsol 82 | disp('_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-_-') 83 | fprintf('Inverse kinematic solution %d\n',i); 84 | disp('Value in degrees'); 85 | disp(rad2deg(joints(i,:))); 86 | disp('Value in radians'); 87 | disp(joints(i,:)); 88 | joints(i,:)=rad2deg(joints(i,:)); 89 | % Run the forward kinematics function for each IK solution 90 | DHMatrix = [ 0 0 d(1) joints(i,1); % 1 0T1 91 | -90 0 d(2) joints(i,2)-90; % 2 1T2 92 | 0 a(2) d(3) joints(i,3); % 3 2T3 93 | 0 a(3) d(4) joints(i,4); % 4 3T4 94 | 0 a(4) d(5) 90; % 4' 4T4' 5 95 | 90 0 0 joints(i,5); % 5 4'T5 6 96 | -90 0 0 -90; % 5' 5T5' 7 97 | 0 a(5) d(6) joints(i,6); % 6 5'T6 8 98 | 0 0 d(7) 0;]; % 7 6T7 9 99 | M=fwdKin(DHMatrix); 100 | % Print robot's tip position 101 | disp('Robot´s tip position in meters:') 102 | fwd_tip_pos_iksol = M{2}{numFrames-1}(:,4).'; 103 | disp(fwd_tip_pos_iksol(1:3)); 104 | % Print robot's tip and end-effector orientation 105 | disp('Robot´s tip orientation in degrees:') 106 | fwd_tip_ori_iksol = RPY(R); 107 | disp(fwd_tip_ori_iksol); 108 | fwd_tip_pose_iksol = [fwd_tip_pos_iksol(1:3) fwd_tip_ori_iksol]; 109 | error=fwd_tip_pose-fwd_tip_pose_iksol; 110 | %% Send errors and computation times to excel 111 | excelValues=[error,t]; %append time to error - send to Excel sheet 112 | writematrix(excelValues,ValFile,'WriteMode','append'); 113 | end 114 | end 115 | 116 | disp('Program ended'); -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # universal-robots-kinematics 2 | 3 | This work was developed in the context of our MSc dissertations: *A Collaborative Work Cell to Improve Ergonomics and Productivity* by João Cunha, and *Human-Like Motion Generation through Waypoints for Collaborative Robots in Industry 4.0* by João Pereira, in which we got to work with the collaborative robotic arm **UR10 e-series**. 4 | 5 | Before producing our kinematics solution, we conducted a comprehensive literature review on the UR robots’ kinematics (which can be consulted in the *"Articles"* folder of this repository) and realised there was a lack of thorough and detailed analysis of their kinematics. Additionally, we found the [Universal Robots’ documentation](https://www.universal-robots.com/articles/ur/parameters-for-calculations-of-kinematics-and-dynamics/) confusing and unclear. 6 | 7 | Thus, the main goal of this work is to provide an **explicit and transparent guide into the UR robots kinematics** (using by reference the UR10 e-series) by expanding on the literature we found and describing every part of our analysis. 8 | 9 | We present a **forward kinematic solution based on the Modified Denavit-Hartenberg convention** and an **inverse kinematic solution based on a geometric analysis**. The code is available in C++ and MATLAB, both with integration with CoppeliaSim. 10 | 11 | 12 | 13 | *** 14 | 15 | ## C++ source code 16 | 17 | The solution was built using Microsoft Visual Studio 2019, and C++ 20 standard. 18 | 19 | ### The `UR class` 20 | 21 | ... 22 | 23 | *** 24 | 25 | ## Dependencies 26 | 27 | ### Eigen 28 | 29 | [Eigen](https://eigen.tuxfamily.org/index.php?title=Main_Page) is a C++ template library for linear algebra: matrices, vectors, numerical solvers, and related algorithms. 30 | 31 | In the context of this solution it is used to simplify the creation of 2D arrays, and matrix operations. 32 | 33 | To use Eigen: 34 | 1. Download the source code from their website; 35 | 2. Extract the contents of the .rar to a directory of your choice (usually directly to your C:/ drive); 36 | 3. On your project properties: 37 | - C/C++ -> Additional Include Directories -> *your Eigen directory e.g. C:\Eigen*; 38 | - now you can include `#include `. 39 | 40 | ### CoppeliaSim 41 | 42 | This isn't necessary if you only want to use the forward and inverse kinematics solutions. 43 | To run the CoppeliaSim integration, you have to install CoppeliaSim. To use their [C++ remote API](https://www.coppeliarobotics.com/helpFiles/en/remoteApiClientSide.htm): 44 | 1. Include the following files in your project directory: extApi.h, extApi.c, extApiPlatform.h, and extApiPlatform.c; which are located in CoppeliaSim's installation directory, under programming/remoteApi; 45 | 2. Define `NON_MATLAB_PARSING` and `MAX_EXT_API_CONNECTIONS=255` (and optionally `DO_NOT_USE_SHARED_MEMORY`) as a preprocessor definition; 46 | 3. Include additional directories remoteApi and include; 47 | 4. If you aren't interested in using the secure version of their calls (like fopen_s), you need to place a definition of `_CRT_SECURE_NO_DEPRECATE` before your included header files. 48 | 49 | *** 50 | 51 | ## Benchmarking 52 | 53 | Benchmarking consists of: 54 | 1. getting [compute times](https://stackoverflow.com/questions/22387586/measuring-execution-time-of-a-function-in-c) of forward and inverse kinematics functions; 55 | 2. getting the error of the solutions. 56 | 57 | The solutions were tested for a set of 100000 randomly generated target tip poses (the scripts were run in a Ryzen 5 3600 CPU at 4.28GHz). 58 | 59 | MATLAB's compute times are 10x slower than with C++, so **the C++ solution is obviously recommended for use**. 60 | 61 | ### Average Computation Times (in seconds) 62 | 63 | ||MATLAB|C++| 64 | |---|---|---| 65 | |Forward Kinematics|1.832571E-05|1.39434e-06| 66 | |Inverse Kinematics|1.612797E-04|1.07403E-05| 67 | 68 | ### Average error 69 | 70 | To compute the average error the following flowchart was followed: 71 | 72 | ![How to get the errors](/Resources/errorSolsFlow.png) 73 | 74 | ||*x*|*y*|*z*|*alpha*|*beta*|*gamma*| 75 | |-|--|---|---|-------|------|-------| 76 | average error|7.45E-14|1.86E-14|7.45E-14|4.14E-06|4.14E-06|4.14E-06| 77 | 78 | **Units in**: *x, y, z* metres, *alpha, beta, gamma* radians. 79 | -------------------------------------------------------------------------------- /Resources/.gitignore: -------------------------------------------------------------------------------- 1 | *.ai 2 | img -------------------------------------------------------------------------------- /Resources/errorSolsFlow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jgocunha/universal-robots-kinematics/52cebc4646821b11e222d079e9ae5ee6bd10a634/Resources/errorSolsFlow.png -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/.gitignore: -------------------------------------------------------------------------------- 1 | # Created by https://www.toptal.com/developers/gitignore/api/visualstudio 2 | # Edit at https://www.toptal.com/developers/gitignore?templates=visualstudio 3 | 4 | ### VisualStudio ### 5 | ## Ignore Visual Studio temporary files, build results, and 6 | ## files generated by popular Visual Studio add-ons. 7 | ## 8 | ## Get latest from https://github.com/github/gitignore/blob/master/VisualStudio.gitignore 9 | 10 | # User-specific files 11 | *.rsuser 12 | *.suo 13 | *.user 14 | *.userosscache 15 | *.sln.docstates 16 | 17 | # User-specific files (MonoDevelop/Xamarin Studio) 18 | *.userprefs 19 | 20 | # Mono auto generated files 21 | mono_crash.* 22 | 23 | # Build results 24 | [Dd]ebug/ 25 | [Dd]ebugPublic/ 26 | [Rr]elease/ 27 | [Rr]eleases/ 28 | x64/ 29 | x86/ 30 | [Ww][Ii][Nn]32/ 31 | [Aa][Rr][Mm]/ 32 | [Aa][Rr][Mm]64/ 33 | bld/ 34 | [Bb]in/ 35 | [Oo]bj/ 36 | [Ll]og/ 37 | [Ll]ogs/ 38 | 39 | # Visual Studio 2015/2017 cache/options directory 40 | .vs/ 41 | # Uncomment if you have tasks that create the project's static files in wwwroot 42 | #wwwroot/ 43 | 44 | # Visual Studio 2017 auto generated files 45 | Generated\ Files/ 46 | 47 | # MSTest test Results 48 | [Tt]est[Rr]esult*/ 49 | [Bb]uild[Ll]og.* 50 | 51 | # NUnit 52 | *.VisualState.xml 53 | TestResult.xml 54 | nunit-*.xml 55 | 56 | # Build Results of an ATL Project 57 | [Dd]ebugPS/ 58 | [Rr]eleasePS/ 59 | dlldata.c 60 | 61 | # Benchmark Results 62 | BenchmarkDotNet.Artifacts/ 63 | 64 | # .NET Core 65 | project.lock.json 66 | project.fragment.lock.json 67 | artifacts/ 68 | 69 | # ASP.NET Scaffolding 70 | ScaffoldingReadMe.txt 71 | 72 | # StyleCop 73 | StyleCopReport.xml 74 | 75 | # Files built by Visual Studio 76 | *_i.c 77 | *_p.c 78 | *_h.h 79 | *.ilk 80 | *.meta 81 | *.obj 82 | *.iobj 83 | *.pch 84 | *.pdb 85 | *.ipdb 86 | *.pgc 87 | *.pgd 88 | *.rsp 89 | *.sbr 90 | *.tlb 91 | *.tli 92 | *.tlh 93 | *.tmp 94 | *.tmp_proj 95 | *_wpftmp.csproj 96 | *.log 97 | *.tlog 98 | *.vspscc 99 | *.vssscc 100 | .builds 101 | *.pidb 102 | *.svclog 103 | *.scc 104 | 105 | # Chutzpah Test files 106 | _Chutzpah* 107 | 108 | # Visual C++ cache files 109 | ipch/ 110 | *.aps 111 | *.ncb 112 | *.opendb 113 | *.opensdf 114 | *.sdf 115 | *.cachefile 116 | *.VC.db 117 | *.VC.VC.opendb 118 | 119 | # Visual Studio profiler 120 | *.psess 121 | *.vsp 122 | *.vspx 123 | *.sap 124 | 125 | # Visual Studio Trace Files 126 | *.e2e 127 | 128 | # TFS 2012 Local Workspace 129 | $tf/ 130 | 131 | # Guidance Automation Toolkit 132 | *.gpState 133 | 134 | # ReSharper is a .NET coding add-in 135 | _ReSharper*/ 136 | *.[Rr]e[Ss]harper 137 | *.DotSettings.user 138 | 139 | # TeamCity is a build add-in 140 | _TeamCity* 141 | 142 | # DotCover is a Code Coverage Tool 143 | *.dotCover 144 | 145 | # AxoCover is a Code Coverage Tool 146 | .axoCover/* 147 | !.axoCover/settings.json 148 | 149 | # Coverlet is a free, cross platform Code Coverage Tool 150 | coverage*.json 151 | coverage*.xml 152 | coverage*.info 153 | 154 | # Visual Studio code coverage results 155 | *.coverage 156 | *.coveragexml 157 | 158 | # NCrunch 159 | _NCrunch_* 160 | .*crunch*.local.xml 161 | nCrunchTemp_* 162 | 163 | # MightyMoose 164 | *.mm.* 165 | AutoTest.Net/ 166 | 167 | # Web workbench (sass) 168 | .sass-cache/ 169 | 170 | # Installshield output folder 171 | [Ee]xpress/ 172 | 173 | # DocProject is a documentation generator add-in 174 | DocProject/buildhelp/ 175 | DocProject/Help/*.HxT 176 | DocProject/Help/*.HxC 177 | DocProject/Help/*.hhc 178 | DocProject/Help/*.hhk 179 | DocProject/Help/*.hhp 180 | DocProject/Help/Html2 181 | DocProject/Help/html 182 | 183 | # Click-Once directory 184 | publish/ 185 | 186 | # Publish Web Output 187 | *.[Pp]ublish.xml 188 | *.azurePubxml 189 | # Note: Comment the next line if you want to checkin your web deploy settings, 190 | # but database connection strings (with potential passwords) will be unencrypted 191 | *.pubxml 192 | *.publishproj 193 | 194 | # Microsoft Azure Web App publish settings. Comment the next line if you want to 195 | # checkin your Azure Web App publish settings, but sensitive information contained 196 | # in these scripts will be unencrypted 197 | PublishScripts/ 198 | 199 | # NuGet Packages 200 | *.nupkg 201 | # NuGet Symbol Packages 202 | *.snupkg 203 | # The packages folder can be ignored because of Package Restore 204 | **/[Pp]ackages/* 205 | # except build/, which is used as an MSBuild target. 206 | !**/[Pp]ackages/build/ 207 | # Uncomment if necessary however generally it will be regenerated when needed 208 | #!**/[Pp]ackages/repositories.config 209 | # NuGet v3's project.json files produces more ignorable files 210 | *.nuget.props 211 | *.nuget.targets 212 | 213 | # Nuget personal access tokens and Credentials 214 | nuget.config 215 | 216 | # Microsoft Azure Build Output 217 | csx/ 218 | *.build.csdef 219 | 220 | # Microsoft Azure Emulator 221 | ecf/ 222 | rcf/ 223 | 224 | # Windows Store app package directories and files 225 | AppPackages/ 226 | BundleArtifacts/ 227 | Package.StoreAssociation.xml 228 | _pkginfo.txt 229 | *.appx 230 | *.appxbundle 231 | *.appxupload 232 | 233 | # Visual Studio cache files 234 | # files ending in .cache can be ignored 235 | *.[Cc]ache 236 | # but keep track of directories ending in .cache 237 | !?*.[Cc]ache/ 238 | 239 | # Others 240 | ClientBin/ 241 | ~$* 242 | *~ 243 | *.dbmdl 244 | *.dbproj.schemaview 245 | *.jfm 246 | *.pfx 247 | *.publishsettings 248 | orleans.codegen.cs 249 | 250 | # Including strong name files can present a security risk 251 | # (https://github.com/github/gitignore/pull/2483#issue-259490424) 252 | #*.snk 253 | 254 | # Since there are multiple workflows, uncomment next line to ignore bower_components 255 | # (https://github.com/github/gitignore/pull/1529#issuecomment-104372622) 256 | #bower_components/ 257 | 258 | # RIA/Silverlight projects 259 | Generated_Code/ 260 | 261 | # Backup & report files from converting an old project file 262 | # to a newer Visual Studio version. Backup files are not needed, 263 | # because we have git ;-) 264 | _UpgradeReport_Files/ 265 | Backup*/ 266 | UpgradeLog*.XML 267 | UpgradeLog*.htm 268 | ServiceFabricBackup/ 269 | *.rptproj.bak 270 | 271 | # SQL Server files 272 | *.mdf 273 | *.ldf 274 | *.ndf 275 | 276 | # Business Intelligence projects 277 | *.rdl.data 278 | *.bim.layout 279 | *.bim_*.settings 280 | *.rptproj.rsuser 281 | *- [Bb]ackup.rdl 282 | *- [Bb]ackup ([0-9]).rdl 283 | *- [Bb]ackup ([0-9][0-9]).rdl 284 | 285 | # Microsoft Fakes 286 | FakesAssemblies/ 287 | 288 | # GhostDoc plugin setting file 289 | *.GhostDoc.xml 290 | 291 | # Node.js Tools for Visual Studio 292 | .ntvs_analysis.dat 293 | node_modules/ 294 | 295 | # Visual Studio 6 build log 296 | *.plg 297 | 298 | # Visual Studio 6 workspace options file 299 | *.opt 300 | 301 | # Visual Studio 6 auto-generated workspace file (contains which files were open etc.) 302 | *.vbw 303 | 304 | # Visual Studio LightSwitch build output 305 | **/*.HTMLClient/GeneratedArtifacts 306 | **/*.DesktopClient/GeneratedArtifacts 307 | **/*.DesktopClient/ModelManifest.xml 308 | **/*.Server/GeneratedArtifacts 309 | **/*.Server/ModelManifest.xml 310 | _Pvt_Extensions 311 | 312 | # Paket dependency manager 313 | .paket/paket.exe 314 | paket-files/ 315 | 316 | # FAKE - F# Make 317 | .fake/ 318 | 319 | # CodeRush personal settings 320 | .cr/personal 321 | 322 | # Python Tools for Visual Studio (PTVS) 323 | __pycache__/ 324 | *.pyc 325 | 326 | # Cake - Uncomment if you are using it 327 | # tools/** 328 | # !tools/packages.config 329 | 330 | # Tabs Studio 331 | *.tss 332 | 333 | # Telerik's JustMock configuration file 334 | *.jmconfig 335 | 336 | # BizTalk build output 337 | *.btp.cs 338 | *.btm.cs 339 | *.odx.cs 340 | *.xsd.cs 341 | 342 | # OpenCover UI analysis results 343 | OpenCover/ 344 | 345 | # Azure Stream Analytics local run output 346 | ASALocalRun/ 347 | 348 | # MSBuild Binary and Structured Log 349 | *.binlog 350 | 351 | # NVidia Nsight GPU debugger configuration file 352 | *.nvuser 353 | 354 | # MFractors (Xamarin productivity tool) working folder 355 | .mfractor/ 356 | 357 | # Local History for Visual Studio 358 | .localhistory/ 359 | 360 | # BeatPulse healthcheck temp database 361 | healthchecksdb 362 | 363 | # Backup folder for Package Reference Convert tool in Visual Studio 2017 364 | MigrationBackup/ 365 | 366 | # Ionide (cross platform F# VS Code tools) working folder 367 | .ionide/ 368 | 369 | # Fody - auto-generated XML schema 370 | FodyWeavers.xsd 371 | 372 | # VS Code files for those working on multiple tools 373 | .vscode/* 374 | !.vscode/settings.json 375 | !.vscode/tasks.json 376 | !.vscode/launch.json 377 | !.vscode/extensions.json 378 | *.code-workspace 379 | 380 | # Local History for Visual Studio Code 381 | .history/ 382 | 383 | # Windows Installer files from build outputs 384 | *.cab 385 | *.msi 386 | *.msix 387 | *.msm 388 | *.msp 389 | 390 | # JetBrains Rider 391 | .idea/ 392 | *.sln.iml 393 | 394 | ### VisualStudio Patch ### 395 | # Additional files built by Visual Studio 396 | 397 | # End of https://www.toptal.com/developers/gitignore/api/visualstudio -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Application/Application.vcxproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | Debug 14 | x64 15 | 16 | 17 | Release 18 | x64 19 | 20 | 21 | 22 | 16.0 23 | Win32Proj 24 | {99e4cfb0-f629-4f58-99ca-5c9a0852e369} 25 | Application 26 | 10.0 27 | 28 | 29 | 30 | Application 31 | true 32 | v142 33 | Unicode 34 | true 35 | 36 | 37 | Application 38 | false 39 | v142 40 | true 41 | Unicode 42 | 43 | 44 | Application 45 | true 46 | v142 47 | Unicode 48 | true 49 | 50 | 51 | Application 52 | false 53 | v142 54 | true 55 | Unicode 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | true 77 | $(SolutionDir)bin\$(Platform)\$(Configuration)\ 78 | $(SolutionDir)bin\inter\$(Platform)\$(Configuration)\ 79 | 80 | 81 | false 82 | $(SolutionDir)bin\$(Platform)\$(Configuration)\ 83 | $(SolutionDir)bin\inter\$(Platform)\$(Configuration)\ 84 | 85 | 86 | true 87 | $(SolutionDir)bin\$(Platform)\$(Configuration)\ 88 | $(SolutionDir)bin\inter\$(Platform)\$(Configuration)\ 89 | 90 | 91 | false 92 | $(SolutionDir)bin\$(Platform)\$(Configuration)\ 93 | $(SolutionDir)bin\inter\$(Platform)\$(Configuration)\ 94 | 95 | 96 | 97 | Level3 98 | true 99 | NON_MATLAB_PARSING;MAX_EXT_API_CONNECTIONS=255;DO_NOT_USE_SHARED_MEMORY 100 | true 101 | stdcpp20 102 | $(SolutionDir)Dependencies\CoppeliaSim\remoteApi;$(SolutionDir)Dependencies\CoppeliaSim\include;C:\Work\Eigen;$(SolutionDir)mathLib\src;$(SolutionDir)universalRobotsKinematics\src 103 | false 104 | 105 | 106 | Console 107 | true 108 | 109 | 110 | 111 | 112 | Level3 113 | true 114 | true 115 | true 116 | NON_MATLAB_PARSING;MAX_EXT_API_CONNECTIONS=255;DO_NOT_USE_SHARED_MEMORY 117 | true 118 | stdcpp20 119 | $(SolutionDir)Dependencies\CoppeliaSim\remoteApi;$(SolutionDir)Dependencies\CoppeliaSim\include;C:\Work\Eigen;$(SolutionDir)mathLib\src;$(SolutionDir)universalRobotsKinematics\src 120 | false 121 | 122 | 123 | Console 124 | true 125 | true 126 | true 127 | 128 | 129 | 130 | 131 | Level3 132 | true 133 | NON_MATLAB_PARSING;MAX_EXT_API_CONNECTIONS=255;DO_NOT_USE_SHARED_MEMORY 134 | true 135 | stdcpp20 136 | $(SolutionDir)Dependencies\CoppeliaSim\remoteApi;$(SolutionDir)Dependencies\CoppeliaSim\include;C:\Work\Eigen;$(SolutionDir)mathLib\src;$(SolutionDir)universalRobotsKinematics\src 137 | false 138 | 139 | 140 | Console 141 | true 142 | 143 | 144 | 145 | 146 | Level3 147 | true 148 | true 149 | true 150 | NON_MATLAB_PARSING;MAX_EXT_API_CONNECTIONS=255;DO_NOT_USE_SHARED_MEMORY 151 | true 152 | stdcpp20 153 | $(SolutionDir)Dependencies\CoppeliaSim\remoteApi;$(SolutionDir)Dependencies\CoppeliaSim\include;C:\Work\Eigen;$(SolutionDir)mathLib\src;$(SolutionDir)universalRobotsKinematics\src 154 | false 155 | 156 | 157 | Console 158 | true 159 | true 160 | true 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | {f9bbb153-b0c4-4dcb-940e-da420bed3376} 173 | 174 | 175 | {7cd73ecd-a445-43e0-a840-adc6d9d2437e} 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Application/Application.vcxproj.filters: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | Source Files 20 | 21 | 22 | Source Files 23 | 24 | 25 | Source Files 26 | 27 | 28 | Source Files 29 | 30 | 31 | Source Files 32 | 33 | 34 | 35 | 36 | Header Files 37 | 38 | 39 | Header Files 40 | 41 | 42 | Header Files 43 | 44 | 45 | Header Files 46 | 47 | 48 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Application/src/benchmarking.cpp: -------------------------------------------------------------------------------- 1 | // benchmarking.cpp 2 | 3 | #include "benchmarking.h" 4 | 5 | 6 | namespace benchmark 7 | { 8 | void runBenchmarkTests(universalRobots::UR& robot) 9 | { 10 | std::cout << "____________________________\nBenchmarking..." << std::endl << std::endl; 11 | 12 | float ikSols[robot.m_numIkSol][robot.m_numDoF] = {}; 13 | universalRobots::pose tipPoseInput = {}; 14 | universalRobots::pose tipPoseOutput = {}; 15 | universalRobots::pose poseError = {}; 16 | universalRobots::pose avgPoseError = {}; 17 | 18 | std::chrono::duration invKin_us; 19 | std::chrono::duration sumInvKin_us; 20 | std::chrono::duration avgInvKin_us; 21 | 22 | std::chrono::duration fwdKin_us; 23 | std::chrono::duration sumFwdKin_us; 24 | std::chrono::duration avgFwdKin_us; 25 | 26 | for (unsigned int i = 0; i < ITERATIONS; i++) 27 | { 28 | universalRobots::pose tipPoseInput = robot.generateRandomReachablePose(); 29 | // generate a random pose 30 | { 31 | timer ikTimer; 32 | robot.inverseKinematics(tipPoseInput, &ikSols); 33 | invKin_us = ikTimer.stop(); // Getting number of microseconds as a double. 34 | } 35 | sumInvKin_us = sumInvKin_us + invKin_us; 36 | 37 | for (unsigned int j = 0; j < robot.m_numIkSol; j++) 38 | { 39 | { 40 | timer fkTimer; 41 | tipPoseOutput = robot.forwardKinematics(ikSols[j]); 42 | fwdKin_us = fkTimer.stop(); // Getting number of microseconds as a double. 43 | } 44 | sumFwdKin_us = sumFwdKin_us + fwdKin_us; 45 | poseError = tipPoseInput - tipPoseOutput; 46 | } 47 | } 48 | avgInvKin_us = sumInvKin_us / ITERATIONS; 49 | avgFwdKin_us = sumFwdKin_us / (ITERATIONS * robot.m_numIkSol); 50 | avgPoseError = poseError / (ITERATIONS * robot.m_numIkSol); 51 | 52 | std::cout << "Generated " << ITERATIONS << " random valid tip poses for " << robot.getRobotType() << "..." << std::endl; 53 | std::cout << "Generated eight inverse kinematic solutions for each tip pose..." << std::endl; 54 | std::cout << "Average IK function execution time: " << avgInvKin_us.count() << "us" << std::endl; 55 | std::cout << "Each IK sol (" << ITERATIONS * robot.m_numIkSol << ") has been run through forward kinematics..." << std::endl; 56 | std::cout << "Average FK function execution time: " << avgFwdKin_us.count() << "us" << std::endl; 57 | std::cout << "error = [tip_pose_input - tip_pose_output]" << std::endl; 58 | std::cout << "average error = " << avgPoseError.m_pos[0] << " " << avgPoseError.m_pos[1] << " " << avgPoseError.m_pos[2] << " " 59 | << avgPoseError.m_eulerAngles[0] << " " << avgPoseError.m_eulerAngles[1] << " " << avgPoseError.m_eulerAngles[2] << std::endl; 60 | } 61 | 62 | std::chrono::microseconds timer::stop() 63 | { 64 | m_endTimePoint = std::chrono::high_resolution_clock::now(); 65 | 66 | auto start = std::chrono::time_point_cast(m_startTimePoint); 67 | auto end = std::chrono::time_point_cast(m_endTimePoint); 68 | 69 | return end - start; 70 | } 71 | 72 | } // namespace benchmark -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Application/src/benchmarking.h: -------------------------------------------------------------------------------- 1 | // benchmarking.h 2 | 3 | #pragma once 4 | 5 | #define ITERATIONS 100000 6 | 7 | #include 8 | #include "universalRobotsKinematics.h" 9 | 10 | // Benchmarking consists of: 11 | // 1. getting compute times of forward and inverse kinematics functions 12 | // 2. getting the error of the solutions 13 | 14 | // https://stackoverflow.com/questions/22387586/measuring-execution-time-of-a-function-in-c 15 | 16 | namespace benchmark 17 | { 18 | class timer 19 | { 20 | private: 21 | std::chrono::time_point m_startTimePoint; 22 | std::chrono::time_point m_endTimePoint; 23 | public: 24 | timer() 25 | { 26 | m_startTimePoint = std::chrono::high_resolution_clock::now(); 27 | } 28 | std::chrono::microseconds stop(); 29 | }; 30 | 31 | void runBenchmarkTests(universalRobots::UR& robot); 32 | 33 | } // namespace benchmark 34 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Application/src/coppeliasimTests.cpp: -------------------------------------------------------------------------------- 1 | // coppeliasimTests.cpp 2 | 3 | #ifndef _DEBUG 4 | 5 | #include 6 | #include "coppeliasimTests.h" 7 | 8 | namespace coppeliaSim 9 | { 10 | void retrieveRobotJointHandles(const int& clientID, const std::string& jointHandleName, const unsigned int& numJoints, int (*out_robotJointHandles)[]) 11 | { 12 | for (unsigned int i = 0; i < numJoints; i++) 13 | simxGetObjectHandle(clientID, (jointHandleName + std::to_string(i + 1)).c_str(), &(*out_robotJointHandles)[i], simx_opmode_oneshot_wait); 14 | } 15 | 16 | void sendRobotTargetJointValues(const int& clientID, const universalRobots::UR& robot, const int (&robotJointHandles)[], const float (&jointValue)[], const unsigned int& waitTime) 17 | { 18 | for (unsigned int j = 0; j < robot.m_numDoF; j++) 19 | simxSetJointTargetPosition(clientID, robotJointHandles[j], jointValue[j], simx_opmode_blocking); 20 | Sleep(waitTime); 21 | } 22 | 23 | std::string getJointHandleName(const universalRobots::UR& robot) 24 | { 25 | switch (robot.getRobotType()) 26 | { 27 | case universalRobots::URtype::UR3: 28 | return "UR3_joint"; 29 | case universalRobots::URtype::UR5: 30 | return "UR5_joint"; 31 | case universalRobots::URtype::UR10: 32 | return "UR10_joint"; 33 | default: 34 | return "UR10_joint"; 35 | } 36 | } 37 | 38 | void runCoppeliaSimTests(universalRobots::UR& robot, const universalRobots::pose& targetTipPose) 39 | { 40 | simxFinish(-1); // Just in case, close all opened connections 41 | const int clientID = simxStart((simxChar*)"127.0.0.1", 19999, true, true, 5000, 5); // start connection 42 | 43 | if (clientID > -1) 44 | { 45 | std::cout << "\nConnected to CoppeliaSim scene!" << std::endl << std::endl; 46 | 47 | const std::string jointHandleName = getJointHandleName(robot); 48 | 49 | // retrieve joint handles from CoppeliaSim 50 | int robotJointHandles[robot.m_numDoF] = {}; 51 | retrieveRobotJointHandles(clientID, jointHandleName, robot.m_numDoF, &robotJointHandles); 52 | 53 | // compute inverse kinematics 54 | float ikSols[robot.m_numIkSol][robot.m_numDoF] = {}; 55 | robot.inverseKinematics(targetTipPose, &ikSols); 56 | 57 | // send joint values to CoppeliaSim 58 | for (unsigned int i = 0; i < robot.m_numIkSol; i++) 59 | { 60 | std::cout << "IK solution " << i + 1 << ": " << mathLib::deg(ikSols[i][0]) << " " << mathLib::deg(ikSols[i][1]) << " " << mathLib::deg(ikSols[i][2]) << " " << 61 | mathLib::deg(ikSols[i][3]) << " " << mathLib::deg(ikSols[i][4]) << " " << mathLib::deg(ikSols[i][5]) << std::endl; 62 | sendRobotTargetJointValues(clientID, robot, robotJointHandles, ikSols[i], 500); 63 | } 64 | 65 | // allow Coppelia to display tip pose 66 | simxSetIntegerSignal(clientID, "showPos", 1, simx_opmode_blocking); 67 | Sleep(1); 68 | simxSetIntegerSignal(clientID, "showPos", 0, simx_opmode_blocking); 69 | } 70 | else 71 | std::cout << "Failed to connect to CoppeliaSim scene!" << std::endl; 72 | 73 | simxFinish(clientID); 74 | } 75 | 76 | } // namespace coppeliaSim 77 | 78 | #endif -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Application/src/coppeliasimTests.h: -------------------------------------------------------------------------------- 1 | // coppeliasimTests.h 2 | 3 | #pragma once 4 | #ifndef _DEBUG 5 | 6 | #include 7 | #include "universalRobotsKinematics.h" 8 | 9 | extern "C" 10 | { 11 | #include "extApi.h" 12 | } 13 | 14 | // https://www.coppeliarobotics.com/helpFiles/en/remoteApiClientSide.htm 15 | // This is not enough!!! You need to include additional directories { remoteApi and include folder } 16 | // and also https://stackoverflow.com/questions/14386/fopen-deprecated-warning 17 | // It looks like Microsoft has deprecated lots of calls which use buffers to improve code security.However, the solutions they're providing aren't portable. Anyway, 18 | // if you aren't interested in using the secure version of their calls (like fopen_s), you need to place a definition of _CRT_SECURE_NO_DEPRECATE before your included header files. 19 | // For example: 20 | 21 | #ifdef _WIN32 22 | #define _CRT_SECURE_NO_DEPRECATE 23 | #endif 24 | 25 | // Run the main application in release mode. 26 | 27 | 28 | namespace coppeliaSim 29 | { 30 | /// 31 | /// Runs the main test. 32 | /// 1st starts the connection. 33 | /// 2nd retrieves the robot's joint handles. 34 | /// 3rd computes the eight inverse kinematics solutions. 35 | /// 4th actuates the target solutions in CoppeliaSim. 36 | /// 37 | /// 38 | void runCoppeliaSimTests(universalRobots::UR& robot, const universalRobots::pose& targetTipPose); 39 | 40 | /// 41 | /// Returns URtype + _joint 42 | /// 43 | /// 44 | /// robotJointHandleName 45 | std::string getJointHandleName(const universalRobots::UR& robot); 46 | 47 | 48 | /// 49 | /// Retreives the joint handles in CoppeliaSim. 50 | /// 51 | /// 52 | /// 53 | /// 54 | /// 55 | void retrieveRobotJointHandles(const int& clientID, const std::string& jointHandleName, const unsigned int& numJoints, int(*out_robotJointHandles)[]); 56 | 57 | /// 58 | /// Sets the joint angles of the robot in CoppeliaSim. 59 | /// 60 | /// 61 | /// 62 | /// 63 | /// 64 | /// 65 | void sendRobotTargetJointValues(const int& clientID, const universalRobots::UR& robot, const int(&robotJointHandles)[], const float(&jointValue)[], const unsigned int& waitTime = 500); 66 | 67 | } // namespace coppeliaSim 68 | 69 | #endif -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Application/src/extApiPlatform.c: -------------------------------------------------------------------------------- 1 | // Shared memory routines are courtesy of Benjamin Navarro 2 | 3 | #include "extApiPlatform.h" 4 | #include 5 | #ifndef DO_NOT_USE_SHARED_MEMORY 6 | #include "shared_memory.h" 7 | #endif 8 | 9 | #ifdef _WIN32 10 | #include 11 | #include 12 | #ifndef QT_COMPIL 13 | #pragma message("Adding library: Winmm.lib") 14 | #pragma comment(lib,"Winmm.lib") 15 | #pragma message("Adding library: Ws2_32.lib") 16 | #pragma comment(lib,"Ws2_32.lib") 17 | #endif 18 | #define MUTEX_HANDLE HANDLE 19 | #define MUTEX_HANDLE_X MUTEX_HANDLE 20 | #define THREAD_ID DWORD 21 | WSADATA _socketWsaData; 22 | #elif defined (__linux) || defined (__APPLE__) 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include /* Defines O_ * constants */ 29 | #include /* Defines mode constants */ 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #define MUTEX_HANDLE pthread_mutex_t 36 | #define MUTEX_HANDLE_X MUTEX_HANDLE* 37 | #define THREAD_ID pthread_t 38 | #define SOCKET int 39 | #define DWORD unsigned long 40 | #define INVALID_SOCKET (-1) 41 | #endif 42 | 43 | #ifndef DO_NOT_USE_SHARED_MEMORY 44 | shared_memory_info_t _shmInfo[MAX_EXT_API_CONNECTIONS]; 45 | #endif 46 | 47 | MUTEX_HANDLE _globalMutex; 48 | 49 | MUTEX_HANDLE _mutex1[MAX_EXT_API_CONNECTIONS]; 50 | MUTEX_HANDLE _mutex1Aux[MAX_EXT_API_CONNECTIONS]; 51 | simxInt _mutex1LockLevel[MAX_EXT_API_CONNECTIONS]; 52 | THREAD_ID _lock1ThreadId[MAX_EXT_API_CONNECTIONS]; 53 | 54 | MUTEX_HANDLE _mutex2[MAX_EXT_API_CONNECTIONS]; 55 | MUTEX_HANDLE _mutex2Aux[MAX_EXT_API_CONNECTIONS]; 56 | simxInt _mutex2LockLevel[MAX_EXT_API_CONNECTIONS]; 57 | THREAD_ID _lock2ThreadId[MAX_EXT_API_CONNECTIONS]; 58 | 59 | SOCKET _socketConn[MAX_EXT_API_CONNECTIONS]; 60 | struct sockaddr_in _socketServer[MAX_EXT_API_CONNECTIONS]; 61 | 62 | simxShort extApi_endianConversionShort(simxShort shortValue) 63 | { /* just used for testing purposes. Endianness is detected on the server side */ 64 | #ifdef ENDIAN_TEST 65 | simxShort retV; 66 | ((char*)&retV)[0]=((char*)&shortValue)[1]; 67 | ((char*)&retV)[1]=((char*)&shortValue)[0]; 68 | return(retV); 69 | #else 70 | return(shortValue); 71 | #endif 72 | } 73 | 74 | simxUShort extApi_endianConversionUShort(simxUShort shortValue) 75 | { /* just used for testing purposes. Endianness is detected on the server side */ 76 | #ifdef ENDIAN_TEST 77 | simxUShort retV; 78 | ((char*)&retV)[0]=((char*)&shortValue)[1]; 79 | ((char*)&retV)[1]=((char*)&shortValue)[0]; 80 | return(retV); 81 | #else 82 | return(shortValue); 83 | #endif 84 | } 85 | 86 | simxInt extApi_endianConversionInt(simxInt intValue) 87 | { /* just used for testing purposes. Endianness is detected on the server side */ 88 | #ifdef ENDIAN_TEST 89 | simxInt retV; 90 | ((char*)&retV)[0]=((char*)&intValue)[3]; 91 | ((char*)&retV)[1]=((char*)&intValue)[2]; 92 | ((char*)&retV)[2]=((char*)&intValue)[1]; 93 | ((char*)&retV)[3]=((char*)&intValue)[0]; 94 | return(retV); 95 | #else 96 | return(intValue); 97 | #endif 98 | } 99 | 100 | simxFloat extApi_endianConversionFloat(simxFloat floatValue) 101 | { /* just used for testing purposes. Endianness is detected on the server side */ 102 | #ifdef ENDIAN_TEST 103 | simxFloat retV; 104 | ((char*)&retV)[0]=((char*)&floatValue)[3]; 105 | ((char*)&retV)[1]=((char*)&floatValue)[2]; 106 | ((char*)&retV)[2]=((char*)&floatValue)[1]; 107 | ((char*)&retV)[3]=((char*)&floatValue)[0]; 108 | return(retV); 109 | #else 110 | return(floatValue); 111 | #endif 112 | } 113 | 114 | simxDouble extApi_endianConversionDouble(simxDouble doubleValue) 115 | { /* just used for testing purposes. Endianness is detected on the server side */ 116 | #ifdef ENDIAN_TEST 117 | simxDouble retV; 118 | ((char*)&retV)[0]=((char*)&doubleValue)[7]; 119 | ((char*)&retV)[1]=((char*)&doubleValue)[6]; 120 | ((char*)&retV)[2]=((char*)&doubleValue)[5]; 121 | ((char*)&retV)[3]=((char*)&doubleValue)[4]; 122 | ((char*)&retV)[4]=((char*)&doubleValue)[3]; 123 | ((char*)&retV)[5]=((char*)&doubleValue)[2]; 124 | ((char*)&retV)[6]=((char*)&doubleValue)[1]; 125 | ((char*)&retV)[7]=((char*)&doubleValue)[0]; 126 | return(retV); 127 | #else 128 | return(doubleValue); 129 | #endif 130 | } 131 | 132 | simxInt extApi_getTimeInMs() 133 | { 134 | #ifdef _WIN32 135 | return(timeGetTime()&0x03ffffff); 136 | #elif defined (__linux) || defined (__APPLE__) 137 | struct timeval tv; 138 | DWORD result=0; 139 | if (gettimeofday(&tv,NULL)==0) 140 | result=(tv.tv_sec*1000+tv.tv_usec/1000)&0x03ffffff; 141 | return(result); 142 | #endif 143 | } 144 | 145 | simxInt extApi_getTimeDiffInMs(simxInt lastTime) 146 | { 147 | simxInt currentTime=extApi_getTimeInMs(); 148 | if (currentTime0) ) 335 | { // Already locked by this thread 336 | _mutex1LockLevel[clientID]++; 337 | _simpleUnlock(_mutex1Aux[clientID]); 338 | return; 339 | } 340 | // First level lock 341 | _simpleUnlock(_mutex1Aux[clientID]); 342 | _simpleLock(_mutex1[clientID]); 343 | _simpleLock(_mutex1Aux[clientID]); 344 | _lock1ThreadId[clientID]=GetCurrentThreadId(); 345 | _mutex1LockLevel[clientID]=1; 346 | _simpleUnlock(_mutex1Aux[clientID]); 347 | #elif defined (__linux) || defined (__APPLE__) 348 | _simpleLock(&_mutex1Aux[clientID]); 349 | if ( (pthread_self()==_lock1ThreadId[clientID]) && (_mutex1LockLevel[clientID]>0) ) 350 | { // Already locked by this thread 351 | _mutex1LockLevel[clientID]++; 352 | _simpleUnlock(&_mutex1Aux[clientID]); 353 | return; 354 | } 355 | // First level lock 356 | _simpleUnlock(&_mutex1Aux[clientID]); 357 | _simpleLock(&_mutex1[clientID]); 358 | _simpleLock(&_mutex1Aux[clientID]); 359 | _lock1ThreadId[clientID]=pthread_self(); 360 | _mutex1LockLevel[clientID]=1; 361 | _simpleUnlock(&_mutex1Aux[clientID]); 362 | #endif 363 | } 364 | 365 | simxVoid extApi_unlockResources(simxInt clientID) 366 | { 367 | #ifdef _WIN32 368 | _simpleLock(_mutex1Aux[clientID]); 369 | _mutex1LockLevel[clientID]--; 370 | if (_mutex1LockLevel[clientID]==0) 371 | { 372 | _simpleUnlock(_mutex1Aux[clientID]); 373 | _simpleUnlock(_mutex1[clientID]); 374 | } 375 | else 376 | _simpleUnlock(_mutex1Aux[clientID]); 377 | #elif defined (__linux) || defined (__APPLE__) 378 | _simpleLock(&_mutex1Aux[clientID]); 379 | _mutex1LockLevel[clientID]--; 380 | if (_mutex1LockLevel[clientID]==0) 381 | { 382 | _simpleUnlock(&_mutex1Aux[clientID]); 383 | _simpleUnlock(&_mutex1[clientID]); 384 | } 385 | else 386 | _simpleUnlock(&_mutex1Aux[clientID]); 387 | #endif 388 | } 389 | 390 | simxVoid extApi_lockSendStart(simxInt clientID) 391 | { 392 | #ifdef _WIN32 393 | _simpleLock(_mutex2Aux[clientID]); 394 | if ( (GetCurrentThreadId()==_lock2ThreadId[clientID]) && (_mutex2LockLevel[clientID]>0) ) 395 | { // Already locked by this thread 396 | _mutex2LockLevel[clientID]++; 397 | _simpleUnlock(_mutex2Aux[clientID]); 398 | return; 399 | } 400 | // First level lock 401 | _simpleUnlock(_mutex2Aux[clientID]); 402 | _simpleLock(_mutex2[clientID]); 403 | _simpleLock(_mutex2Aux[clientID]); 404 | _lock2ThreadId[clientID]=GetCurrentThreadId(); 405 | _mutex2LockLevel[clientID]=1; 406 | _simpleUnlock(_mutex2Aux[clientID]); 407 | #elif defined (__linux) || defined (__APPLE__) 408 | _simpleLock(&_mutex2Aux[clientID]); 409 | if ( (pthread_self()==_lock2ThreadId[clientID]) && (_mutex2LockLevel[clientID]>0) ) 410 | { // Already locked by this thread 411 | _mutex2LockLevel[clientID]++; 412 | _simpleUnlock(&_mutex2Aux[clientID]); 413 | return; 414 | } 415 | // First level lock 416 | _simpleUnlock(&_mutex2Aux[clientID]); 417 | _simpleLock(&_mutex2[clientID]); 418 | _simpleLock(&_mutex2Aux[clientID]); 419 | _lock2ThreadId[clientID]=pthread_self(); 420 | _mutex2LockLevel[clientID]=1; 421 | _simpleUnlock(&_mutex2Aux[clientID]); 422 | #endif 423 | } 424 | 425 | simxVoid extApi_unlockSendStart(simxInt clientID) 426 | { 427 | #ifdef _WIN32 428 | _simpleLock(_mutex2Aux[clientID]); 429 | _mutex2LockLevel[clientID]--; 430 | if (_mutex2LockLevel[clientID]==0) 431 | { 432 | _simpleUnlock(_mutex2Aux[clientID]); 433 | _simpleUnlock(_mutex2[clientID]); 434 | } 435 | else 436 | _simpleUnlock(_mutex2Aux[clientID]); 437 | #elif defined (__linux) || defined (__APPLE__) 438 | _simpleLock(&_mutex2Aux[clientID]); 439 | _mutex2LockLevel[clientID]--; 440 | if (_mutex2LockLevel[clientID]==0) 441 | { 442 | _simpleUnlock(&_mutex2Aux[clientID]); 443 | _simpleUnlock(&_mutex2[clientID]); 444 | } 445 | else 446 | _simpleUnlock(&_mutex2Aux[clientID]); 447 | #endif 448 | } 449 | 450 | simxUChar extApi_launchThread(SIMX_THREAD_RET_TYPE(*startAddress)(simxVoid*)) 451 | { 452 | #ifdef _WIN32 453 | return(_beginthread(startAddress,0,0)!=0); 454 | #elif defined (__linux) || defined (__APPLE__) 455 | pthread_t th; 456 | return (pthread_create(&th,NULL,startAddress,NULL) == 0); 457 | #endif 458 | } 459 | 460 | simxVoid extApi_endThread() 461 | { 462 | #ifndef _WIN32 463 | pthread_detach(pthread_self()); 464 | #endif 465 | } 466 | 467 | simxUChar extApi_connectToServer_socket(simxInt clientID,const simxChar* theConnectionAddress,simxInt theConnectionPort) 468 | { /* return 1: success */ 469 | /* struct hostent *hp; 470 | simxUInt addr; */ 471 | #ifdef _WIN32 472 | if (WSAStartup(0x101,&_socketWsaData)!=0) 473 | return(0); 474 | #endif 475 | _socketConn[clientID]=socket(AF_INET,SOCK_STREAM,IPPROTO_TCP); 476 | if(_socketConn[clientID]==INVALID_SOCKET) 477 | { 478 | #ifdef _WIN32 479 | WSACleanup(); 480 | #endif 481 | return(0); 482 | } 483 | /* 484 | Following code can be problematic since some IP Addresses can't be resolved: 485 | if (inet_addr(theConnectionAddress)==INADDR_NONE) 486 | hp=gethostbyname(theConnectionAddress); 487 | else 488 | { 489 | addr=inet_addr(theConnectionAddress); 490 | hp=gethostbyaddr((char*)&addr,sizeof(addr),AF_INET); 491 | } 492 | if(hp==NULL) 493 | { 494 | #ifdef _WIN32 495 | closesocket(_socketConn[clientID]); 496 | WSACleanup(); 497 | #elif defined (__linux) || defined (__APPLE__) 498 | close(_socketConn[clientID]); 499 | #endif 500 | return(0); 501 | } 502 | _socketServer[clientID].sin_addr.s_addr=*((unsigned long*)hp->h_addr); 503 | */ 504 | 505 | /* Above code replaced with: */ 506 | _socketServer[clientID].sin_addr.s_addr=inet_addr(theConnectionAddress); 507 | 508 | 509 | 510 | _socketServer[clientID].sin_family=AF_INET; 511 | _socketServer[clientID].sin_port=htons(theConnectionPort); 512 | if(connect(_socketConn[clientID],(struct sockaddr*)&_socketServer[clientID],sizeof(_socketServer[clientID]))) 513 | { 514 | #ifdef _WIN32 515 | closesocket(_socketConn[clientID]); 516 | WSACleanup(); 517 | #elif defined (__linux) || defined (__APPLE__) 518 | close(_socketConn[clientID]); 519 | #endif 520 | return(0); 521 | } 522 | return(1); 523 | } 524 | 525 | simxVoid extApi_cleanUp_socket(simxInt clientID) 526 | { 527 | #ifdef _WIN32 528 | closesocket(_socketConn[clientID]); 529 | WSACleanup(); 530 | #elif defined (__linux) || defined (__APPLE__) 531 | close(_socketConn[clientID]); 532 | #endif 533 | } 534 | 535 | simxInt extApi_send_socket(simxInt clientID,const simxUChar* data,simxInt dataLength) 536 | { 537 | return(send(_socketConn[clientID],(char*)data,dataLength,0)); 538 | } 539 | 540 | simxInt extApi_recv_socket(simxInt clientID,simxUChar* data,simxInt maxDataLength) 541 | { 542 | return(recv(_socketConn[clientID],(char*)data,maxDataLength,0)); 543 | } 544 | 545 | 546 | 547 | // Following shared memory routines courtesy of Benjamin Navarro 548 | simxUChar extApi_connectToServer_sharedMem(simxInt clientID,simxInt theConnectionPort) 549 | { /* return 1: success */ 550 | #ifndef DO_NOT_USE_SHARED_MEMORY 551 | theConnectionPort=-theConnectionPort; 552 | shared_memory_info_t shared_memory_info; 553 | set_shared_memory_name(&shared_memory_info, theConnectionPort); 554 | set_shared_memory_size(&shared_memory_info, 2); 555 | 556 | if(open_shared_memory(&shared_memory_info)) 557 | { 558 | if(map_shared_memory(&shared_memory_info)) 559 | { 560 | if(shared_memory_info.buffer[0]==0) 561 | { 562 | set_shared_memory_size(&shared_memory_info, ((simxInt*)(shared_memory_info.buffer+1))[0]); 563 | 564 | if(map_shared_memory(&shared_memory_info) == false) { 565 | fprintf(stderr, "Connect (%d): Failed to remap the shared memory \"%s\"\n", clientID,shared_memory_info.name); 566 | } 567 | fflush(stderr); 568 | 569 | shared_memory_info.buffer[5]=0; /* client has nothing to send */ 570 | shared_memory_info.buffer[0]=1; /* connected */ 571 | 572 | _shmInfo[clientID] = shared_memory_info; 573 | return 1; 574 | } 575 | } 576 | else 577 | { 578 | fprintf(stderr, "Connect (%d): Failed to map the shared memory \"%s\"\n", clientID, shared_memory_info.name); 579 | } 580 | } 581 | else 582 | { 583 | fprintf(stderr, "Connect (%d): Failed to open the shared memory \"%s\"\n", clientID, shared_memory_info.name); 584 | } 585 | fflush(stderr); 586 | destroy_shared_memory(&shared_memory_info); 587 | #endif 588 | return 0; 589 | } 590 | 591 | simxVoid extApi_cleanUp_sharedMem(simxInt clientID) 592 | { 593 | #ifndef DO_NOT_USE_SHARED_MEMORY 594 | _shmInfo[clientID].buffer[0] = 0; 595 | #if defined (__linux) || defined (__APPLE__) 596 | // Doesn't work on Windows, don't known why... 597 | if(unmap_shared_memory(&_shmInfo[clientID]) == false) { 598 | fprintf(stderr, "Clean up (%d): Failed to unmap the shared memory \"%s\"\n", clientID, _shmInfo[clientID].name); 599 | } 600 | #endif 601 | if(close_shared_memory(&_shmInfo[clientID]) == false) { 602 | fprintf(stderr, "Clean up (%d): Failed to close the shared memory \"%s\"\n", clientID, _shmInfo[clientID].name); 603 | } 604 | fflush(stderr); 605 | #endif 606 | } 607 | 608 | simxInt extApi_send_sharedMem(simxInt clientID,const simxUChar* data,simxInt dataLength) 609 | { 610 | #ifndef DO_NOT_USE_SHARED_MEMORY 611 | simxInt startTime; 612 | simxInt off=0; 613 | simxInt initDataLength=dataLength; 614 | if (dataLength==0) 615 | return(0); 616 | startTime=extApi_getTimeInMs(); 617 | if (_shmInfo[clientID].buffer[0] != 1) 618 | { 619 | return(0); 620 | } 621 | 622 | while (dataLength > 0) 623 | { 624 | /* Wait for previous data to be gone: */ 625 | while (_shmInfo[clientID].buffer[5] != 0) 626 | { 627 | if (extApi_getTimeDiffInMs(startTime)>1000) 628 | { 629 | return(0); 630 | } 631 | } 632 | 633 | /* ok, we can send the data: */ 634 | if (dataLength <= _shmInfo[clientID].size) 635 | { /* we can send the data in one shot: */ 636 | memcpy(_shmInfo[clientID].buffer+_shmInfo[clientID].header_size, data+off, dataLength); 637 | ((int*)(_shmInfo[clientID].buffer+6))[0] = dataLength; 638 | ((int*)(_shmInfo[clientID].buffer+6))[1] = _shmInfo[clientID].header_size; 639 | ((int*)(_shmInfo[clientID].buffer+6))[2] = initDataLength; 640 | dataLength=0; 641 | } 642 | else 643 | { /* just send a smaller part first: */ 644 | memcpy(_shmInfo[clientID].buffer+_shmInfo[clientID].header_size, data+off, _shmInfo[clientID].size); 645 | ((int*)(_shmInfo[clientID].buffer+6))[0] = _shmInfo[clientID].size; 646 | ((int*)(_shmInfo[clientID].buffer+6))[1] = _shmInfo[clientID].header_size; 647 | ((int*)(_shmInfo[clientID].buffer+6))[2] = initDataLength; 648 | dataLength -= _shmInfo[clientID].size; 649 | off += _shmInfo[clientID].size; 650 | } 651 | _shmInfo[clientID].buffer[5] = 1; /* client has something to send! */ 652 | } 653 | return(initDataLength); 654 | #else 655 | return(0); 656 | #endif 657 | } 658 | 659 | simxUChar* extApi_recv_sharedMem(simxInt clientID,simxInt* dataLength) 660 | { 661 | #ifndef DO_NOT_USE_SHARED_MEMORY 662 | simxInt startT; 663 | simxInt l=0; 664 | simxInt off=0; 665 | simxInt retDataOff=0; 666 | simxUChar* retData=0; 667 | simxInt totalLength=-1; 668 | if (_shmInfo[clientID].buffer[0] != 1) 669 | { /* we are not connected anymore */ 670 | return(0); 671 | } 672 | 673 | startT=extApi_getTimeInMs(); 674 | while (retDataOff!=totalLength) 675 | { 676 | /* Wait for data: */ 677 | while (_shmInfo[clientID].buffer[5]!=2) 678 | { 679 | if (extApi_getTimeDiffInMs(startT)>1000) 680 | { 681 | return(0); 682 | } 683 | } 684 | /* ok, data is there! */ 685 | /* Read the data with correct length: */ 686 | l=((int*)(_shmInfo[clientID].buffer+6))[0]; 687 | off=((int*)(_shmInfo[clientID].buffer+6))[1]; 688 | totalLength=((int*)(_shmInfo[clientID].buffer+6))[2]; 689 | if (retData==0) 690 | retData=extApi_allocateBuffer(totalLength); 691 | memcpy(retData+retDataOff,_shmInfo[clientID].buffer+off,l); 692 | retDataOff=retDataOff+l; 693 | /* Tell the other side we have read that part and additional parts could be sent (if present): */ 694 | _shmInfo[clientID].buffer[5]=0; 695 | } 696 | dataLength[0]=retDataOff; 697 | return(retData); 698 | #else 699 | return(0); 700 | #endif 701 | } 702 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Application/src/extApiPlatform.h: -------------------------------------------------------------------------------- 1 | #ifndef _EXTAPIPLATFORM__ 2 | #define _EXTAPIPLATFORM__ 3 | 4 | #define _CRT_SECURE_NO_DEPRECATE 5 | 6 | 7 | #ifdef __cplusplus 8 | extern "C" { 9 | #endif 10 | #ifdef _WIN32 11 | /* on older win compilers stdint.h can be missing */ 12 | typedef unsigned char uint8_t; 13 | typedef short int16_t; 14 | typedef unsigned short uint16_t; 15 | typedef int int32_t; 16 | typedef unsigned int uint32_t; 17 | #else 18 | #include 19 | #endif 20 | 21 | #define SOCKET_MAX_PACKET_SIZE 1300 /* in bytes. Keep between 200 and 30000 */ 22 | #define SOCKET_HEADER_LENGTH 6 /* WORD0=1 (to detect endianness), WORD1=packetSize, WORD2=packetsLeftToRead */ 23 | #define SOCKET_TIMEOUT_READ 10000 /* in ms */ 24 | 25 | typedef char simxChar; /* always 1 byte */ 26 | typedef uint8_t simxUChar; /* always 1 byte */ 27 | typedef int16_t simxShort; /* always 2 bytes */ 28 | typedef uint16_t simxUShort; /* always 2 bytes */ 29 | typedef int32_t simxInt; /* always 4 bytes */ 30 | typedef uint32_t simxUInt; /* always 4 bytes */ 31 | typedef float simxFloat; /* always 4 bytes */ 32 | typedef double simxDouble; /* always 8 bytes */ 33 | typedef void simxVoid; 34 | 35 | 36 | #ifdef _WIN32 37 | #define SIMX_THREAD_RET_TYPE simxVoid 38 | #define SIMX_THREAD_RET_LINE return 39 | #elif defined (__linux) || defined (__APPLE__) 40 | #define SIMX_THREAD_RET_TYPE simxVoid* 41 | #define SIMX_THREAD_RET_LINE return(0) 42 | #endif 43 | 44 | 45 | /* Following functions only needed for testing endianness robustness */ 46 | simxShort extApi_endianConversionShort(simxShort shortValue); 47 | simxUShort extApi_endianConversionUShort(simxUShort shortValue); 48 | simxInt extApi_endianConversionInt(simxInt intValue); 49 | simxFloat extApi_endianConversionFloat(simxFloat floatValue); 50 | simxDouble extApi_endianConversionDouble(simxDouble floatValue); 51 | 52 | /* Following functions might be platform specific */ 53 | simxFloat extApi_getFloatFromPtr(const simxUChar* ptr); 54 | simxInt extApi_getIntFromPtr(const simxUChar* ptr); 55 | simxUChar* extApi_allocateBuffer(simxInt bufferSize); 56 | simxVoid extApi_releaseBuffer(simxUChar* buffer); 57 | simxVoid extApi_createMutexes(simxInt clientID); 58 | simxVoid extApi_deleteMutexes(simxInt clientID); 59 | simxVoid extApi_lockResources(simxInt clientID); 60 | simxVoid extApi_unlockResources(simxInt clientID); 61 | simxVoid extApi_lockSendStart(simxInt clientID); 62 | simxVoid extApi_unlockSendStart(simxInt clientID); 63 | simxVoid extApi_createGlobalMutex(); 64 | simxVoid extApi_deleteGlobalMutex(); 65 | simxVoid extApi_globalSimpleLock(); 66 | simxVoid extApi_globalSimpleUnlock(); 67 | simxInt extApi_getTimeInMs(); 68 | simxInt extApi_getTimeDiffInMs(simxInt lastTime); 69 | simxVoid extApi_initRand(); 70 | simxFloat extApi_rand(); 71 | simxVoid extApi_sleepMs(simxInt ms); 72 | simxVoid extApi_switchThread(); 73 | simxUChar extApi_areStringsSame(const simxChar* str1,const simxChar* str2); 74 | simxInt extApi_getStringLength(const simxChar* str); 75 | simxUChar* extApi_readFile(const simxChar* fileName,simxInt* len); 76 | simxUChar extApi_launchThread(SIMX_THREAD_RET_TYPE (*startAddress)(simxVoid*)); 77 | simxVoid extApi_endThread(); 78 | simxUChar extApi_connectToServer_socket(simxInt clientID,const simxChar* theConnectionAddress,simxInt theConnectionPort); 79 | simxVoid extApi_cleanUp_socket(simxInt clientID); 80 | simxInt extApi_send_socket(simxInt clientID,const simxUChar* data,simxInt dataLength); 81 | simxInt extApi_recv_socket(simxInt clientID,simxUChar* data,simxInt maxDataLength); 82 | 83 | simxUChar extApi_connectToServer_sharedMem(simxInt clientID,simxInt theConnectionPort); 84 | simxVoid extApi_cleanUp_sharedMem(simxInt clientID); 85 | simxInt extApi_send_sharedMem(simxInt clientID,const simxUChar* data,simxInt dataLength); 86 | simxUChar* extApi_recv_sharedMem(simxInt clientID,simxInt* dataLength); 87 | 88 | #ifdef __cplusplus 89 | } 90 | #endif 91 | 92 | #endif /* _EXTAPIPLATFORM__ */ 93 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Application/src/main.cpp: -------------------------------------------------------------------------------- 1 | // main.cpp 2 | 3 | #include 4 | #include "universalRobotsKinematics.h" 5 | #include "coppeliaSimTests.h" 6 | #include "benchmarking.h" 7 | 8 | 9 | int main() 10 | { 11 | // instatiate a UR object 12 | // 13 | universalRobots::UR robot(universalRobots::URtype::UR5); 14 | 15 | // test different target joint values 16 | // 17 | const float targetJointValues[robot.m_numDoF] = { mathLib::rad(23), mathLib::rad(345), mathLib::rad(78), mathLib::rad(66), mathLib::rad(77), mathLib::rad(12) }; 18 | 19 | // compute forward kinematics and update robot's tip pose 20 | // 21 | robot.forwardKinematics(targetJointValues); 22 | 23 | std::cout << robot << std::endl; 24 | 25 | // test different target tip poses 26 | // 27 | const universalRobots::pose targetTipPose = robot.forwardKinematics(targetJointValues); 28 | 29 | // compute inverse kinematics 30 | // 31 | float ikSols[robot.m_numIkSol][robot.m_numDoF] = {}; 32 | robot.inverseKinematics(targetTipPose, &ikSols); 33 | 34 | std::cout << "Inverse kinematics" << std::endl; 35 | for (unsigned int i = 0; i < robot.m_numIkSol; i++) 36 | std::cout << "IK solution " << i + 1 << ": " << mathLib::deg(ikSols[i][0]) << " " << mathLib::deg(ikSols[i][1]) << " " << mathLib::deg(ikSols[i][2]) << " " << 37 | mathLib::deg(ikSols[i][3]) << " " << mathLib::deg(ikSols[i][4]) << " " << mathLib::deg(ikSols[i][5]) << std::endl; 38 | 39 | // coppeliasim test 40 | // 41 | coppeliaSim::runCoppeliaSimTests(robot, targetTipPose); 42 | 43 | // benchmarking 44 | // 45 | universalRobots::UR benchmarkRobot(universalRobots::URtype::UR5); 46 | benchmark::runBenchmarkTests(benchmarkRobot); 47 | 48 | std::cin.get(); 49 | 50 | return 0; 51 | } -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/include/license.txt: -------------------------------------------------------------------------------- 1 | All the files in each directory are licensed under the terms of the 2 | license text contained at the beginning of the same file, or if not 3 | present, under the terms of the license file (license, license.txt, 4 | copying, etc.) contained in the same directory if it is present, or 5 | under the same terms of the files in the parent directory otherwise. 6 | 7 | ----------------------------------------------------------------------- 8 | include files, based on the include files from Coppelia Robotics AG on 9 | September 2019 10 | 11 | Copyright (C) 2006-2021 Coppelia Robotics AG / Marc Freese 12 | Copyright (C) 2019 Robot Nordic ApS 13 | 14 | All rights reserved. 15 | 16 | BSD License 2.0 17 | 18 | Redistribution and use in source and binary forms, with or without 19 | modification, are permitted provided that the following conditions are met: 20 | * Redistributions of source code must retain the above copyright 21 | notice, this list of conditions and the following disclaimer. 22 | * Redistributions in binary form must reproduce the above copyright 23 | notice, this list of conditions and the following disclaimer in the 24 | documentation and/or other materials provided with the distribution. 25 | * Neither the name of Coppelia Robotics nor the 26 | names of its contributors may be used to endorse or promote products 27 | derived from this software without specific prior written permission. 28 | 29 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 30 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 31 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 32 | DISCLAIMED. IN NO EVENT SHALL COPPELIA ROBOTICS AG BE LIABLE FOR ANY 33 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 34 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 35 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 36 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 37 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 38 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 39 | ----------------------------------------------------------------------- 40 | 41 | The shared memory part of the remote API is courtesy of Benjamin Navarro 42 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/include/luaFunctionData.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "luaFunctionDataItem.h" 4 | #include "simLib.h" 5 | 6 | #define SIM_LUA_ARG_NIL_ALLOWED (65536) 7 | 8 | class CLuaFunctionData 9 | { 10 | public: 11 | CLuaFunctionData(); 12 | virtual ~CLuaFunctionData(); 13 | 14 | //------------------------ 15 | static void getInputDataForFunctionRegistration(const int* dat,std::vector& outDat); 16 | bool readDataFromLua(const SLuaCallBack* p,const int* expectedArguments,int requiredArgumentCount,const char* functionName); 17 | std::vector* getInDataPtr(); 18 | void pushOutData(const CLuaFunctionDataItem& dataItem); 19 | void writeDataToLua(SLuaCallBack* p); 20 | //------------------------ 21 | 22 | 23 | //------------------------ 24 | void pushOutData_luaFunctionCall(const CLuaFunctionDataItem& dataItem); 25 | void writeDataToLua_luaFunctionCall(SLuaCallBack* p,const int* expectedArguments); 26 | bool readDataFromLua_luaFunctionCall(const SLuaCallBack* p,const int* expectedArguments,int requiredArgumentCount,const char* functionName); 27 | std::vector* getOutDataPtr_luaFunctionCall(); 28 | void releaseBuffers_luaFunctionCall(SLuaCallBack* p); 29 | //------------------------ 30 | 31 | 32 | 33 | protected: 34 | std::vector _inData; 35 | std::vector _outData; 36 | }; 37 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/include/luaFunctionDataItem.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | class CLuaFunctionDataItem 7 | { 8 | public: 9 | CLuaFunctionDataItem(); 10 | CLuaFunctionDataItem(bool v); 11 | CLuaFunctionDataItem(int v); 12 | CLuaFunctionDataItem(float v); 13 | CLuaFunctionDataItem(double v); 14 | CLuaFunctionDataItem(const std::string& v); 15 | CLuaFunctionDataItem(const char* bufferPtr,unsigned int bufferLength); 16 | 17 | CLuaFunctionDataItem(const std::vector& v); 18 | CLuaFunctionDataItem(const std::vector& v); 19 | CLuaFunctionDataItem(const std::vector& v); 20 | CLuaFunctionDataItem(const std::vector& v); 21 | CLuaFunctionDataItem(const std::vector& v); 22 | 23 | virtual ~CLuaFunctionDataItem(); 24 | 25 | bool isTable(); 26 | int getType(); 27 | void setNilTable(int size); 28 | int getNilTableSize(); 29 | 30 | std::vector boolData; 31 | std::vector intData; 32 | std::vector floatData; 33 | std::vector doubleData; 34 | std::vector stringData; 35 | 36 | protected: 37 | int _nilTableSize; 38 | bool _isTable; 39 | int _type; // -1=nil,0=bool,1=int,2=float,3=string,4=buffer,5=double 40 | }; 41 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/include/scriptFunctionData.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "scriptFunctionDataItem.h" 4 | #include "simLib.h" 5 | 6 | #define SIM_SCRIPT_ARG_NULL_ALLOWED (65536) 7 | 8 | class CScriptFunctionData 9 | { 10 | public: 11 | CScriptFunctionData(); 12 | virtual ~CScriptFunctionData(); 13 | 14 | //------------------------ 15 | // Use following to read args coming from a script call to this plugin, and to return args back to a script 16 | bool readDataFromStack(int stackHandle,const int* expectedArguments,int requiredArgumentCount,const char* functionName); 17 | std::vector* getInDataPtr(); 18 | void pushOutData(const CScriptFunctionDataItem& dataItem); 19 | void writeDataToStack(int stackHandle); 20 | //------------------------ 21 | 22 | 23 | //------------------------ 24 | // Use following to write args for a script function call, and to read the return values from that script function call 25 | void pushOutData_scriptFunctionCall(const CScriptFunctionDataItem& dataItem); 26 | void writeDataToStack_scriptFunctionCall(int stackHandle); 27 | bool readDataFromStack_scriptFunctionCall(int stackHandle,const int* expectedArguments,int requiredArgumentCount,const char* functionName); 28 | std::vector* getOutDataPtr_scriptFunctionCall(); 29 | //------------------------ 30 | 31 | protected: 32 | bool _readData(int stack,const int* expectedArguments,int requiredArgumentCount,const char* functionName,const char* argumentText1,const char* argumentText2,std::vector& inOutData); 33 | void _writeData(int stack,std::vector& inOutData); 34 | 35 | std::vector _inData; 36 | std::vector _outData; 37 | }; 38 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/include/scriptFunctionDataItem.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | class CScriptFunctionDataItem 7 | { 8 | public: 9 | CScriptFunctionDataItem(); 10 | CScriptFunctionDataItem(bool v); 11 | CScriptFunctionDataItem(int v); 12 | CScriptFunctionDataItem(float v); 13 | CScriptFunctionDataItem(double v); 14 | CScriptFunctionDataItem(const std::string& str); 15 | CScriptFunctionDataItem(const char* str); 16 | CScriptFunctionDataItem(const char* bufferPtr,unsigned int bufferLength); 17 | 18 | CScriptFunctionDataItem(const std::vector& v); 19 | CScriptFunctionDataItem(const std::vector& v); 20 | CScriptFunctionDataItem(const std::vector& v); 21 | CScriptFunctionDataItem(const std::vector& v); 22 | CScriptFunctionDataItem(const std::vector& v); 23 | 24 | virtual ~CScriptFunctionDataItem(); 25 | 26 | bool isTable(); 27 | int getType(); 28 | void setNilTable(int size); 29 | int getNilTableSize(); 30 | 31 | std::vector boolData; 32 | std::vector int32Data; 33 | std::vector floatData; 34 | std::vector doubleData; 35 | std::vector stringData; 36 | 37 | protected: 38 | int _nilTableSize; 39 | bool _isTable; 40 | int _type; // -1=nil,0=bool,1=int32,2=float,3=string,4=buffer,5=double 41 | }; 42 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/include/shared_memory.h: -------------------------------------------------------------------------------- 1 | // This file, and generally the shared memory part of the remote API 2 | // are courtesy of Benjamin Navarro 3 | 4 | #pragma once 5 | 6 | #include 7 | #include 8 | 9 | #ifdef _WIN32 10 | typedef void* shm_handle_t; 11 | #elif defined (__linux) || defined (__APPLE__) 12 | typedef int shm_handle_t; 13 | #endif /* _WIN32 */ 14 | 15 | typedef struct 16 | { 17 | char name[28]; 18 | shm_handle_t handle; 19 | size_t header_size; 20 | size_t size; 21 | unsigned char* buffer; 22 | } shared_memory_info_t; 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | bool create_shared_memory(shared_memory_info_t* info); 29 | bool destroy_shared_memory(shared_memory_info_t* info); 30 | bool open_shared_memory(shared_memory_info_t* info); 31 | bool close_shared_memory(shared_memory_info_t* info); 32 | bool map_shared_memory(shared_memory_info_t* info); 33 | bool unmap_shared_memory(shared_memory_info_t* info); 34 | void set_shared_memory_name(shared_memory_info_t* info, int identifier); 35 | void set_shared_memory_size(shared_memory_info_t* info, size_t size); 36 | bool is_valid_shared_memory_handle(shared_memory_info_t* info); 37 | 38 | #ifdef __cplusplus 39 | } 40 | #endif -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/include/simTypes.h: -------------------------------------------------------------------------------- 1 | #if !defined(SIMTYPES_INCLUDED_) 2 | #define SIMTYPES_INCLUDED_ 3 | 4 | #include 5 | 6 | // Various types used in the interface functions: 7 | typedef unsigned char simBool; 8 | typedef char simChar; 9 | typedef int simInt; 10 | typedef float simFloat; 11 | typedef double simDouble; 12 | typedef void simVoid; 13 | typedef unsigned char simUChar; 14 | typedef unsigned int simUInt; 15 | typedef unsigned long long int simUInt64; 16 | typedef long long int simInt64; 17 | 18 | struct SScriptCallBack 19 | { 20 | simInt objectID; 21 | simInt scriptID; 22 | simInt stackID; 23 | simChar waitUntilZero; /* do not use */ 24 | simChar* raiseErrorWithMessage; /* do not use */ 25 | simChar* source; 26 | simInt line; 27 | }; 28 | 29 | struct SShapeVizInfo 30 | { 31 | simFloat* vertices; 32 | simInt verticesSize; 33 | simInt* indices; 34 | simInt indicesSize; 35 | simFloat shadingAngle; 36 | simFloat* normals; 37 | simFloat colors[9]; 38 | simChar* texture; /*rgba*/ 39 | simInt textureId; 40 | simInt textureRes[2]; 41 | simFloat* textureCoords; 42 | simInt textureApplyMode; 43 | simInt textureOptions; /* not just textures options */ 44 | }; 45 | 46 | struct SLuaCallBack 47 | { 48 | simInt objectID; 49 | simBool* inputBool; 50 | simInt* inputInt; 51 | simFloat* inputFloat; 52 | simChar* inputChar; 53 | simInt inputArgCount; 54 | simInt* inputArgTypeAndSize; 55 | simBool* outputBool; 56 | simInt* outputInt; 57 | simFloat* outputFloat; 58 | simChar* outputChar; 59 | simInt outputArgCount; 60 | simInt* outputArgTypeAndSize; 61 | simChar waitUntilZero; 62 | simChar* inputCharBuff; 63 | simChar* outputCharBuff; 64 | simInt scriptID; 65 | simDouble* inputDouble; 66 | simDouble* outputDouble; 67 | }; 68 | 69 | struct SSyncMsg 70 | { 71 | unsigned char msg; 72 | void* data; 73 | size_t dataSize; 74 | }; 75 | 76 | struct SSyncRt 77 | { 78 | unsigned char objTypes[3]; 79 | int objHandles[3]; 80 | }; 81 | 82 | typedef int (*contactCallback)(int,int,int,int*,float*); 83 | typedef int (*jointCtrlCallback)(int,int,int,const int*,const float*,float*); 84 | 85 | #endif // !defined(SIMTYPES_INCLUDED_) 86 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/include/socketInConnection.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #ifdef _WIN32 7 | #include 8 | #include 9 | #include 10 | #ifndef QT_COMPIL 11 | #pragma message("Adding library: Winmm.lib") 12 | #pragma comment(lib,"Winmm.lib") 13 | #pragma message("Adding library: Ws2_32.lib") 14 | #pragma comment(lib,"Ws2_32.lib") 15 | #endif 16 | typedef timeval _timeval; 17 | typedef int _socklen; 18 | #elif defined (__linux) || defined (__APPLE__) 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #define SOCKET int 26 | #define INVALID_SOCKET (-1) 27 | typedef socklen_t _socklen; 28 | #endif 29 | 30 | 31 | class CSocketInConnection 32 | { 33 | public: 34 | CSocketInConnection(int theConnectionPort,unsigned short maxPacketSize=250,char headerID1=59,char headerID2=57); 35 | virtual ~CSocketInConnection(); 36 | 37 | bool connectToClient(); 38 | char* receiveData(int& dataSize); 39 | bool replyToReceivedData(char* data,int dataSize); 40 | 41 | std::string getConnectedMachineIP(); 42 | 43 | protected: 44 | bool _sendSimplePacket(char* packet,int packetLength,unsigned short packetsLeft); 45 | int _receiveSimplePacket(std::vector& packet); 46 | 47 | unsigned int _getTimeInMs(); 48 | unsigned int _getTimeDiffInMs(unsigned int lastTime); 49 | 50 | 51 | SOCKET _socketServer; 52 | SOCKET _socketClient; 53 | struct sockaddr_in _socketLocal; 54 | 55 | fd_set _socketTheSet; 56 | #ifdef _WIN32 57 | WSADATA _socketWsaData; 58 | #endif /* _WIN32 */ 59 | 60 | int _socketConnectionPort; 61 | bool _socketConnectWasOk; 62 | std::string _socketConnectedMachineIP; 63 | 64 | char _headerByte1; 65 | char _headerByte2; 66 | unsigned short _maxPacketSize; 67 | }; 68 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/include/socketOutConnection.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #ifdef _WIN32 7 | #include 8 | #include 9 | #ifndef QT_COMPIL 10 | #pragma message("Adding library: Winmm.lib") 11 | #pragma comment(lib,"Winmm.lib") 12 | #pragma message("Adding library: Ws2_32.lib") 13 | #pragma comment(lib,"Ws2_32.lib") 14 | #endif 15 | #elif defined (__linux) || defined (__APPLE__) 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #define SOCKET int 22 | #define INVALID_SOCKET (-1) 23 | #endif 24 | 25 | class CSocketOutConnection 26 | { 27 | public: 28 | CSocketOutConnection(const char* theConnectionAddress,int theConnectionPort,unsigned short maxPacketSize=250,char headerID1=59,char headerID2=57); 29 | virtual ~CSocketOutConnection(); 30 | 31 | int connectToServer(); 32 | bool sendData(const char* data,int dataSize); 33 | char* receiveReplyData(int& dataSize); 34 | 35 | protected: 36 | bool _sendSimplePacket(const char* packet,int packetLength,unsigned short packetsLeft); 37 | int _receiveSimplePacket(std::vector& packet); 38 | 39 | int _getTimeInMs(); 40 | int _getTimeDiffInMs(int lastTime); 41 | 42 | std::string _socketConnectionAddress; 43 | int _socketConnectionPort; 44 | 45 | #ifdef _WIN32 46 | WSADATA _socketWsaData; 47 | #else 48 | 49 | #endif 50 | SOCKET _socketConn; 51 | struct sockaddr_in _socketServer; 52 | 53 | char _headerByte1; 54 | char _headerByte2; 55 | unsigned short _maxPacketSize; 56 | }; 57 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/include/stack/stackArray.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "stackObject.h" 5 | 6 | class CStackMap; 7 | 8 | class CStackArray : public CStackObject 9 | { 10 | public: 11 | CStackArray(); 12 | virtual ~CStackArray(); 13 | 14 | std::string toString() const; 15 | 16 | bool buildFromStack(int stackId); 17 | void buildOntoStack(int stackId); 18 | 19 | void appendTopStackItem(int stackId); 20 | 21 | CStackObject* copyYourself(); 22 | 23 | bool pushNull(); 24 | bool pushBool(bool d); 25 | bool pushFloat(float d); 26 | bool pushDouble(double d); 27 | bool pushInt(int d); 28 | bool pushString(const std::string& d); 29 | bool pushString(const char* d,size_t bufferLength); 30 | bool pushArray(CStackArray* arr); 31 | bool pushMap(CStackMap* map); 32 | bool setDoubleArray(const double* d,size_t l); 33 | bool setIntArray(const int* d,size_t l); 34 | 35 | bool isNumberArray(); 36 | size_t getSize(); 37 | 38 | bool isNull(size_t index); 39 | bool isBool(size_t index); 40 | bool isNumber(size_t index); 41 | bool isString(size_t index); 42 | bool isArray(size_t index,size_t minSize=0); 43 | bool isMap(size_t index); 44 | 45 | void setCircularRef(); 46 | bool isCircularRef(); 47 | 48 | bool getBool(size_t index); 49 | float getFloat(size_t index); 50 | double getDouble(size_t index); 51 | int getInt(size_t index); 52 | std::string getString(size_t index); 53 | CStackArray* getArray(size_t index); 54 | CStackMap* getMap(size_t index); 55 | 56 | const std::vector* getObjects(); 57 | const std::vector* getDoubles(); 58 | const std::vector* getInts(); 59 | const double* getDoublePointer(); 60 | const int* getIntPointer(); 61 | 62 | protected: 63 | std::vector _objectValues; 64 | std::vector _doubleValues; 65 | std::vector _intValues; 66 | bool _circularRef; 67 | }; 68 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/include/stack/stackBool.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "stackObject.h" 4 | 5 | class CStackBool : public CStackObject 6 | { 7 | public: 8 | CStackBool(bool theValue); 9 | virtual ~CStackBool(); 10 | 11 | std::string toString() const; 12 | 13 | CStackObject* copyYourself(); 14 | 15 | bool getValue(); 16 | void setValue(bool theValue); 17 | 18 | protected: 19 | bool _value; 20 | }; 21 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/include/stack/stackMap.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "stackObject.h" 4 | #include 5 | 6 | class CStackArray; 7 | 8 | class CStackMap : public CStackObject 9 | { 10 | public: 11 | CStackMap(); 12 | virtual ~CStackMap(); 13 | 14 | std::string toString() const; 15 | 16 | void appendTopStackItem(const char* key,int stackId); 17 | void appendTopStackItem(int key,int stackId); 18 | void appendTopStackItem(bool key,int stackId); 19 | 20 | CStackObject* copyYourself(); 21 | 22 | void setNull(const char* key); 23 | void setBool(const char* key,bool d); 24 | void setFloat(const char* key,float d); 25 | void setDouble(const char* key,double d); 26 | void setInt(const char* key,int d); 27 | void setString(const char* key,const std::string& d); 28 | void setString(const char* key,const char* d,size_t bufferLength); 29 | void setArray(const char* key,CStackArray* arr); 30 | void setMap(const char* key,CStackMap* map); 31 | 32 | void setNull(int key); 33 | void setBool(int key,bool d); 34 | void setFloat(int key,float d); 35 | void setDouble(int key,double d); 36 | void setInt(int key,int d); 37 | void setString(int key,const std::string& d); 38 | void setString(int key,const char* d,size_t bufferLength); 39 | void setArray(int key,CStackArray* arr); 40 | void setMap(int key,CStackMap* map); 41 | 42 | void setNull(bool key); 43 | void setBool(bool key,bool d); 44 | void setFloat(bool key,float d); 45 | void setDouble(bool key,double d); 46 | void setInt(bool key,int d); 47 | void setString(bool key,const std::string& d); 48 | void setString(bool key,const char* d,size_t bufferLength); 49 | void setArray(bool key,CStackArray* arr); 50 | void setMap(bool key,CStackMap* map); 51 | 52 | bool isKeyPresent(const char* key); 53 | bool isNull(const char* key); 54 | bool isBool(const char* key); 55 | bool isNumber(const char* key); 56 | bool isString(const char* key); 57 | bool isArray(const char* key,size_t minSize=0); 58 | bool isMap(const char* key); 59 | 60 | bool isKeyPresent(int key); 61 | bool isNull(int key); 62 | bool isBool(int key); 63 | bool isNumber(int key); 64 | bool isString(int key); 65 | bool isArray(int key,size_t minSize=0); 66 | bool isMap(int key); 67 | 68 | bool isKeyPresent(bool key); 69 | bool isNull(bool key); 70 | bool isBool(bool key); 71 | bool isNumber(bool key); 72 | bool isString(bool key); 73 | bool isArray(bool key,size_t minSize=0); 74 | bool isMap(bool key); 75 | 76 | bool getBool(const char* key); 77 | float getFloat(const char* key); 78 | double getDouble(const char* key); 79 | int getInt(const char* key); 80 | std::string getString(const char* key); 81 | CStackArray* getArray(const char* key); 82 | CStackMap* getMap(const char* key); 83 | 84 | bool getBool(int key); 85 | float getFloat(int key); 86 | double getDouble(int key); 87 | int getInt(int key); 88 | std::string getString(int key); 89 | CStackArray* getArray(int key); 90 | CStackMap* getMap(int key); 91 | 92 | bool getBool(bool key); 93 | float getFloat(bool key); 94 | double getDouble(bool key); 95 | int getInt(bool key); 96 | std::string getString(bool key); 97 | CStackArray* getArray(bool key); 98 | CStackMap* getMap(bool key); 99 | 100 | bool contains(const char* key,int theType=-1,size_t theMinSizeIfArray=0,bool onlyNumbersInArray=false); 101 | bool contains(int key,int theType=-1,size_t theMinSizeIfArray=0,bool onlyNumbersInArray=false); 102 | bool contains(bool key,int theType=-1,size_t theMinSizeIfArray=0,bool onlyNumbersInArray=false); 103 | 104 | std::map* getKeyValuePairs(); 105 | std::map* getKeyValuePairsKStr(); 106 | std::map* getKeyValuePairsKInt(); 107 | std::map* getKeyValuePairsKBool(); 108 | 109 | protected: 110 | void _remove(const char* key); 111 | void _remove(int key); 112 | void _remove(bool key); 113 | 114 | std::map _objectValuesKStr; 115 | std::map _objectValuesKInt; 116 | std::map _objectValuesKBool; 117 | }; 118 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/include/stack/stackNull.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "stackObject.h" 4 | 5 | class CStackNull : public CStackObject 6 | { 7 | public: 8 | CStackNull(); 9 | virtual ~CStackNull(); 10 | 11 | std::string toString() const; 12 | 13 | CStackObject* copyYourself(); 14 | }; 15 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/include/stack/stackNumber.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "stackObject.h" 4 | 5 | class CStackNumber : public CStackObject 6 | { 7 | public: 8 | CStackNumber(double n); 9 | virtual ~CStackNumber(); 10 | 11 | std::string toString() const; 12 | 13 | CStackObject* copyYourself(); 14 | 15 | float getFloatValue(); 16 | int getIntValue(); 17 | long getLongValue(); 18 | double getValue(); 19 | void setFloatValue(float n); 20 | void setIntValue(int n); 21 | void setLongValue(long n); 22 | void setValue(double n); 23 | 24 | protected: 25 | double _value; 26 | }; 27 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/include/stack/stackObject.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "simLib.h" 3 | #include 4 | 5 | enum { STACK_NULL=0, 6 | STACK_NUMBER, 7 | STACK_BOOL, 8 | STACK_STRING, 9 | STACK_ARRAY, 10 | STACK_MAP 11 | }; 12 | 13 | class CStackNull; 14 | class CStackNumber; 15 | class CStackBool; 16 | class CStackString; 17 | class CStackArray; 18 | class CStackMap; 19 | 20 | class CStackObject 21 | { 22 | public: 23 | CStackObject(); 24 | virtual ~CStackObject(); 25 | 26 | virtual CStackObject* copyYourself(); 27 | 28 | int getObjectType() const; 29 | 30 | static void buildItemOntoStack(int stackId,CStackObject* obj); 31 | static CStackObject* buildItemFromTopStackPosition(int stackId); 32 | 33 | CStackNull* asNull(); 34 | CStackNumber* asNumber(); 35 | CStackBool* asBool(); 36 | CStackString* asString(); 37 | CStackArray* asArray(); 38 | CStackMap* asMap(); 39 | 40 | virtual std::string toString() const = 0; 41 | std::string getObjectTypeString() const; 42 | 43 | protected: 44 | int _objectType; 45 | }; 46 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/include/stack/stackString.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "stackObject.h" 4 | 5 | class CStackString : public CStackObject 6 | { 7 | public: 8 | CStackString(const char* str,int l); 9 | virtual ~CStackString(); 10 | 11 | std::string toString() const; 12 | 13 | CStackObject* copyYourself(); 14 | 15 | std::string getValue(); 16 | void setValue(const char* str,int l); 17 | 18 | protected: 19 | std::string _value; 20 | }; 21 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/remoteApi/extApiInternal.h: -------------------------------------------------------------------------------- 1 | #ifndef __EXTAPIINTERNAL_ 2 | #define __EXTAPIINTERNAL_ 3 | 4 | #include "extApiPlatform.h" 5 | 6 | #define SIMX_INIT_BUFF_SIZE 500 7 | #define SIMX_MIN_BUFF_INCR 500 8 | 9 | #define _REPLY_WAIT_TIMEOUT_IN_MS 5000 10 | #define _MIN_SPLIT_AMOUNT_IN_BYTES 100 11 | 12 | /* Out buffer for messages */ 13 | simxUChar* _messageToSend[MAX_EXT_API_CONNECTIONS]; 14 | simxInt _messageToSend_bufferSize[MAX_EXT_API_CONNECTIONS]; 15 | simxInt _messageToSend_dataSize[MAX_EXT_API_CONNECTIONS]; 16 | 17 | /* Temp buffer for split commands to send */ 18 | simxUChar* _splitCommandsToSend[MAX_EXT_API_CONNECTIONS]; 19 | simxInt _splitCommandsToSend_bufferSize[MAX_EXT_API_CONNECTIONS]; 20 | simxInt _splitCommandsToSend_dataSize[MAX_EXT_API_CONNECTIONS]; 21 | 22 | /* In buffer for messages */ 23 | simxUChar* _messageReceived[MAX_EXT_API_CONNECTIONS]; 24 | simxInt _messageReceived_bufferSize[MAX_EXT_API_CONNECTIONS]; 25 | simxInt _messageReceived_dataSize[MAX_EXT_API_CONNECTIONS]; 26 | 27 | /* Temp buffer for split commands received */ 28 | simxUChar* _splitCommandsReceived[MAX_EXT_API_CONNECTIONS]; 29 | simxInt _splitCommandsReceived_bufferSize[MAX_EXT_API_CONNECTIONS]; 30 | simxInt _splitCommandsReceived_dataSize[MAX_EXT_API_CONNECTIONS]; 31 | 32 | /* Temp buffer for last fetched command */ 33 | simxUChar* _commandReceived[MAX_EXT_API_CONNECTIONS]; 34 | simxInt _commandReceived_bufferSize[MAX_EXT_API_CONNECTIONS]; 35 | simxInt _commandReceived_simulationTime[MAX_EXT_API_CONNECTIONS]; 36 | 37 | /* Other variables */ 38 | simxInt _nextConnectionID[MAX_EXT_API_CONNECTIONS]; 39 | simxInt _replyWaitTimeoutInMs[MAX_EXT_API_CONNECTIONS]; 40 | 41 | simxInt _connectionPort[MAX_EXT_API_CONNECTIONS]; 42 | simxChar* _connectionIP[MAX_EXT_API_CONNECTIONS]; 43 | 44 | simxInt _minCommunicationDelay[MAX_EXT_API_CONNECTIONS]; 45 | simxUChar _communicationThreadRunning[MAX_EXT_API_CONNECTIONS]; 46 | simxInt _nextMessageIDToSend[MAX_EXT_API_CONNECTIONS]; 47 | simxInt _waitBeforeSendingAgainWhenMessageIDArrived[MAX_EXT_API_CONNECTIONS]; 48 | simxInt _lastReceivedMessageID[MAX_EXT_API_CONNECTIONS]; 49 | simxInt _connectionID[MAX_EXT_API_CONNECTIONS]; 50 | const simxChar* _tempConnectionAddress[MAX_EXT_API_CONNECTIONS]; 51 | simxInt _tempConnectionPort[MAX_EXT_API_CONNECTIONS]; 52 | simxUChar _tempDoNotReconnectOnceDisconnected[MAX_EXT_API_CONNECTIONS]; 53 | 54 | simxUChar* _tmpBuffer[MAX_EXT_API_CONNECTIONS]; 55 | 56 | 57 | #endif /* __EXTAPIINTERNAL_ */ 58 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/remoteApi/extApiPlatform.h: -------------------------------------------------------------------------------- 1 | #ifndef _EXTAPIPLATFORM__ 2 | #define _EXTAPIPLATFORM__ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | #ifdef _WIN32 8 | /* on older win compilers stdint.h can be missing */ 9 | typedef unsigned char uint8_t; 10 | typedef short int16_t; 11 | typedef unsigned short uint16_t; 12 | typedef int int32_t; 13 | typedef unsigned int uint32_t; 14 | #else 15 | #include 16 | #endif 17 | 18 | #define SOCKET_MAX_PACKET_SIZE 1300 /* in bytes. Keep between 200 and 30000 */ 19 | #define SOCKET_HEADER_LENGTH 6 /* WORD0=1 (to detect endianness), WORD1=packetSize, WORD2=packetsLeftToRead */ 20 | #define SOCKET_TIMEOUT_READ 10000 /* in ms */ 21 | 22 | typedef char simxChar; /* always 1 byte */ 23 | typedef uint8_t simxUChar; /* always 1 byte */ 24 | typedef int16_t simxShort; /* always 2 bytes */ 25 | typedef uint16_t simxUShort; /* always 2 bytes */ 26 | typedef int32_t simxInt; /* always 4 bytes */ 27 | typedef uint32_t simxUInt; /* always 4 bytes */ 28 | typedef float simxFloat; /* always 4 bytes */ 29 | typedef double simxDouble; /* always 8 bytes */ 30 | typedef void simxVoid; 31 | 32 | 33 | #ifdef _WIN32 34 | #define SIMX_THREAD_RET_TYPE simxVoid 35 | #define SIMX_THREAD_RET_LINE return 36 | #elif defined (__linux) || defined (__APPLE__) 37 | #define SIMX_THREAD_RET_TYPE simxVoid* 38 | #define SIMX_THREAD_RET_LINE return(0) 39 | #endif 40 | 41 | 42 | /* Following functions only needed for testing endianness robustness */ 43 | simxShort extApi_endianConversionShort(simxShort shortValue); 44 | simxUShort extApi_endianConversionUShort(simxUShort shortValue); 45 | simxInt extApi_endianConversionInt(simxInt intValue); 46 | simxFloat extApi_endianConversionFloat(simxFloat floatValue); 47 | simxDouble extApi_endianConversionDouble(simxDouble floatValue); 48 | 49 | /* Following functions might be platform specific */ 50 | simxFloat extApi_getFloatFromPtr(const simxUChar* ptr); 51 | simxInt extApi_getIntFromPtr(const simxUChar* ptr); 52 | simxUChar* extApi_allocateBuffer(simxInt bufferSize); 53 | simxVoid extApi_releaseBuffer(simxUChar* buffer); 54 | simxVoid extApi_createMutexes(simxInt clientID); 55 | simxVoid extApi_deleteMutexes(simxInt clientID); 56 | simxVoid extApi_lockResources(simxInt clientID); 57 | simxVoid extApi_unlockResources(simxInt clientID); 58 | simxVoid extApi_lockSendStart(simxInt clientID); 59 | simxVoid extApi_unlockSendStart(simxInt clientID); 60 | simxVoid extApi_createGlobalMutex(); 61 | simxVoid extApi_deleteGlobalMutex(); 62 | simxVoid extApi_globalSimpleLock(); 63 | simxVoid extApi_globalSimpleUnlock(); 64 | simxInt extApi_getTimeInMs(); 65 | simxInt extApi_getTimeDiffInMs(simxInt lastTime); 66 | simxVoid extApi_initRand(); 67 | simxFloat extApi_rand(); 68 | simxVoid extApi_sleepMs(simxInt ms); 69 | simxVoid extApi_switchThread(); 70 | simxUChar extApi_areStringsSame(const simxChar* str1,const simxChar* str2); 71 | simxInt extApi_getStringLength(const simxChar* str); 72 | simxUChar* extApi_readFile(const simxChar* fileName,simxInt* len); 73 | simxUChar extApi_launchThread(SIMX_THREAD_RET_TYPE (*startAddress)(simxVoid*)); 74 | simxVoid extApi_endThread(); 75 | simxUChar extApi_connectToServer_socket(simxInt clientID,const simxChar* theConnectionAddress,simxInt theConnectionPort); 76 | simxVoid extApi_cleanUp_socket(simxInt clientID); 77 | simxInt extApi_send_socket(simxInt clientID,const simxUChar* data,simxInt dataLength); 78 | simxInt extApi_recv_socket(simxInt clientID,simxUChar* data,simxInt maxDataLength); 79 | 80 | simxUChar extApi_connectToServer_sharedMem(simxInt clientID,simxInt theConnectionPort); 81 | simxVoid extApi_cleanUp_sharedMem(simxInt clientID); 82 | simxInt extApi_send_sharedMem(simxInt clientID,const simxUChar* data,simxInt dataLength); 83 | simxUChar* extApi_recv_sharedMem(simxInt clientID,simxInt* dataLength); 84 | 85 | #ifdef __cplusplus 86 | } 87 | #endif 88 | 89 | #endif /* _EXTAPIPLATFORM__ */ 90 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/Dependencies/CoppeliaSim/remoteApi/license.txt: -------------------------------------------------------------------------------- 1 | All the files in each directory are licensed under the terms of the 2 | license text contained at the beginning of the same file, or if not 3 | present, under the terms of the license file (license, license.txt, 4 | copying, etc.) contained in the same directory if it is present, or 5 | under the same terms of the files in the parent directory otherwise. 6 | 7 | ----------------------------------------------------------------------- 8 | remoteApi, based on remoteApi from Coppelia Robotics AG on September 2019 9 | 10 | Copyright (C) 2012-2021 Coppelia Robotics AG / Marc Freese 11 | Copyright (C) 2019 Robot Nordic ApS 12 | 13 | All rights reserved. 14 | 15 | BSD License 2.0 16 | 17 | Redistribution and use in source and binary forms, with or without 18 | modification, are permitted provided that the following conditions are met: 19 | * Redistributions of source code must retain the above copyright 20 | notice, this list of conditions and the following disclaimer. 21 | * Redistributions in binary form must reproduce the above copyright 22 | notice, this list of conditions and the following disclaimer in the 23 | documentation and/or other materials provided with the distribution. 24 | * Neither the name of Coppelia Robotics nor the 25 | names of its contributors may be used to endorse or promote products 26 | derived from this software without specific prior written permission. 27 | 28 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 29 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 30 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 31 | DISCLAIMED. IN NO EVENT SHALL COPPELIA ROBOTICS AG BE LIABLE FOR ANY 32 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 33 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 34 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 35 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 36 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 37 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 38 | ----------------------------------------------------------------------- 39 | 40 | The shared memory part of the remote API is courtesy of Benjamin Navarro 41 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/mathLib/mathLib.vcxproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | Debug 14 | x64 15 | 16 | 17 | Release 18 | x64 19 | 20 | 21 | 22 | 16.0 23 | Win32Proj 24 | {f9bbb153-b0c4-4dcb-940e-da420bed3376} 25 | mathLib 26 | 10.0 27 | 28 | 29 | 30 | StaticLibrary 31 | true 32 | v142 33 | Unicode 34 | 35 | 36 | StaticLibrary 37 | false 38 | v142 39 | true 40 | Unicode 41 | 42 | 43 | StaticLibrary 44 | true 45 | v142 46 | Unicode 47 | 48 | 49 | StaticLibrary 50 | false 51 | v142 52 | true 53 | Unicode 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | true 75 | $(SolutionDir)bin\$(Platform)\$(Configuration)\ 76 | $(SolutionDir)bin\inter\$(Platform)\$(Configuration)\ 77 | 78 | 79 | false 80 | $(SolutionDir)bin\$(Platform)\$(Configuration)\ 81 | $(SolutionDir)bin\inter\$(Platform)\$(Configuration)\ 82 | 83 | 84 | true 85 | $(SolutionDir)bin\$(Platform)\$(Configuration)\ 86 | $(SolutionDir)bin\inter\$(Platform)\$(Configuration)\ 87 | 88 | 89 | false 90 | $(SolutionDir)bin\$(Platform)\$(Configuration)\ 91 | $(SolutionDir)bin\inter\$(Platform)\$(Configuration)\ 92 | 93 | 94 | 95 | Level3 96 | true 97 | WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) 98 | true 99 | stdcpp20 100 | C:\Work\Eigen;%(AdditionalIncludeDirectories) 101 | 102 | 103 | Console 104 | true 105 | 106 | 107 | 108 | 109 | Level3 110 | true 111 | true 112 | true 113 | WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 114 | true 115 | stdcpp20 116 | C:\Work\Eigen;%(AdditionalIncludeDirectories) 117 | 118 | 119 | Console 120 | true 121 | true 122 | true 123 | 124 | 125 | 126 | 127 | Level3 128 | true 129 | _DEBUG;_CONSOLE;%(PreprocessorDefinitions) 130 | true 131 | stdcpp20 132 | C:\Work\Eigen;%(AdditionalIncludeDirectories) 133 | 134 | 135 | Console 136 | true 137 | 138 | 139 | 140 | 141 | Level3 142 | true 143 | true 144 | true 145 | NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 146 | true 147 | stdcpp20 148 | C:\Work\Eigen;%(AdditionalIncludeDirectories) 149 | 150 | 151 | Console 152 | true 153 | true 154 | true 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/mathLib/mathLib.vcxproj.filters: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | Source Files 20 | 21 | 22 | 23 | 24 | Header Files 25 | 26 | 27 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/mathLib/src/mathLib.cpp: -------------------------------------------------------------------------------- 1 | // mathLib.cpp 2 | 3 | #include "mathLib.h" 4 | 5 | 6 | namespace mathLib 7 | { 8 | float rad(const float& degree) 9 | { 10 | return (degree * std::numbers::pi_v / 180);// C++20 11 | //return (degree * PI / 180); //non-compiler dependant 12 | } 13 | 14 | float deg(const float& rad) 15 | { 16 | return (rad * 180 / std::numbers::pi_v);// C++20 17 | //return (rad * 180 / PI); //non-compiler dependant 18 | } 19 | 20 | 21 | Eigen::Matrix4f calcTransformationMatrix(const Eigen::RowVector4f& DHparams) 22 | { 23 | Eigen::Matrix4f individualTransformationMatrix; 24 | individualTransformationMatrix << cos(DHparams[3]), -sin(DHparams[3]), 0, DHparams[1], 25 | (sin(DHparams[3]) * cos(DHparams[0])), (cos(DHparams[3]) * cos(DHparams[0])), -sin(DHparams[0]), (-sin(DHparams[0]) * DHparams[2]), 26 | (sin(DHparams[3]) * sin(DHparams[0])), (cos(DHparams[3]) * sin(DHparams[0])), cos(DHparams[0]), (cos(DHparams[0]) * DHparams[2]), 27 | 0, 0, 0, 1; 28 | 29 | return individualTransformationMatrix; 30 | } 31 | 32 | 33 | } //namespace mathLib 34 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/mathLib/src/mathLib.h: -------------------------------------------------------------------------------- 1 | // mathLib.h 2 | 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | namespace mathLib 9 | { 10 | //const double PI = std::atan(1.0) * 4; 11 | 12 | float rad(const float& degree); 13 | float deg(const float& rad); 14 | 15 | Eigen::Matrix4f calcTransformationMatrix(const Eigen::RowVector4f& DHparams); 16 | 17 | } //namespace mathLib 18 | 19 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/universalRobotsKinematics.sln: -------------------------------------------------------------------------------- 1 |  2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio Version 16 4 | VisualStudioVersion = 16.0.31702.278 5 | MinimumVisualStudioVersion = 10.0.40219.1 6 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "universalRobotsKinematics", "universalRobotsKinematics\universalRobotsKinematics.vcxproj", "{7CD73ECD-A445-43E0-A840-ADC6D9D2437E}" 7 | EndProject 8 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "Application", "Application\Application.vcxproj", "{99E4CFB0-F629-4F58-99CA-5C9A0852E369}" 9 | EndProject 10 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "mathLib", "mathLib\mathLib.vcxproj", "{F9BBB153-B0C4-4DCB-940E-DA420BED3376}" 11 | EndProject 12 | Global 13 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 14 | Debug|x64 = Debug|x64 15 | Debug|x86 = Debug|x86 16 | Release|x64 = Release|x64 17 | Release|x86 = Release|x86 18 | EndGlobalSection 19 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 20 | {7CD73ECD-A445-43E0-A840-ADC6D9D2437E}.Debug|x64.ActiveCfg = Debug|x64 21 | {7CD73ECD-A445-43E0-A840-ADC6D9D2437E}.Debug|x64.Build.0 = Debug|x64 22 | {7CD73ECD-A445-43E0-A840-ADC6D9D2437E}.Debug|x86.ActiveCfg = Debug|Win32 23 | {7CD73ECD-A445-43E0-A840-ADC6D9D2437E}.Debug|x86.Build.0 = Debug|Win32 24 | {7CD73ECD-A445-43E0-A840-ADC6D9D2437E}.Release|x64.ActiveCfg = Release|x64 25 | {7CD73ECD-A445-43E0-A840-ADC6D9D2437E}.Release|x64.Build.0 = Release|x64 26 | {7CD73ECD-A445-43E0-A840-ADC6D9D2437E}.Release|x86.ActiveCfg = Release|Win32 27 | {7CD73ECD-A445-43E0-A840-ADC6D9D2437E}.Release|x86.Build.0 = Release|Win32 28 | {99E4CFB0-F629-4F58-99CA-5C9A0852E369}.Debug|x64.ActiveCfg = Debug|x64 29 | {99E4CFB0-F629-4F58-99CA-5C9A0852E369}.Debug|x64.Build.0 = Debug|x64 30 | {99E4CFB0-F629-4F58-99CA-5C9A0852E369}.Debug|x86.ActiveCfg = Debug|Win32 31 | {99E4CFB0-F629-4F58-99CA-5C9A0852E369}.Debug|x86.Build.0 = Debug|Win32 32 | {99E4CFB0-F629-4F58-99CA-5C9A0852E369}.Release|x64.ActiveCfg = Release|x64 33 | {99E4CFB0-F629-4F58-99CA-5C9A0852E369}.Release|x64.Build.0 = Release|x64 34 | {99E4CFB0-F629-4F58-99CA-5C9A0852E369}.Release|x86.ActiveCfg = Release|Win32 35 | {99E4CFB0-F629-4F58-99CA-5C9A0852E369}.Release|x86.Build.0 = Release|Win32 36 | {F9BBB153-B0C4-4DCB-940E-DA420BED3376}.Debug|x64.ActiveCfg = Debug|x64 37 | {F9BBB153-B0C4-4DCB-940E-DA420BED3376}.Debug|x64.Build.0 = Debug|x64 38 | {F9BBB153-B0C4-4DCB-940E-DA420BED3376}.Debug|x86.ActiveCfg = Debug|Win32 39 | {F9BBB153-B0C4-4DCB-940E-DA420BED3376}.Debug|x86.Build.0 = Debug|Win32 40 | {F9BBB153-B0C4-4DCB-940E-DA420BED3376}.Release|x64.ActiveCfg = Release|x64 41 | {F9BBB153-B0C4-4DCB-940E-DA420BED3376}.Release|x64.Build.0 = Release|x64 42 | {F9BBB153-B0C4-4DCB-940E-DA420BED3376}.Release|x86.ActiveCfg = Release|Win32 43 | {F9BBB153-B0C4-4DCB-940E-DA420BED3376}.Release|x86.Build.0 = Release|Win32 44 | EndGlobalSection 45 | GlobalSection(SolutionProperties) = preSolution 46 | HideSolutionNode = FALSE 47 | EndGlobalSection 48 | GlobalSection(ExtensibilityGlobals) = postSolution 49 | SolutionGuid = {01232494-567A-4A92-B970-F538C30140FF} 50 | EndGlobalSection 51 | EndGlobal 52 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/universalRobotsKinematics/src/universalRobotsKinematics.cpp: -------------------------------------------------------------------------------- 1 | // universalRobotsKinematics.cpp 2 | 3 | #include "universalRobotsKinematics.h" 4 | #include 5 | 6 | 7 | namespace universalRobots 8 | { 9 | /// 10 | /// Constructor. User is only allowed to specify whether there is an end-effector and its translation to the tip. 11 | /// Example usage: 12 | /// universalRobots::UR robot_one(); Robot does not have an end-effector. 13 | /// universalRobots::UR robot_one(true, 0.15f); End-effector translated 0.15 meters from the robot's tip. 14 | /// 15 | /// 16 | /// 17 | UR::UR(const URtype& robotType, const bool& endEffector, const float& endEffectorDimension) 18 | : m_type(robotType), m_endEffector(endEffector) 19 | { 20 | switch (robotType) 21 | { 22 | case universalRobots::URtype::UR3: 23 | setTransZ(UR3_LINK_DIMENSIONS_d); 24 | setTransX(UR3_LINK_DIMENSIONS_a); 25 | break; 26 | case universalRobots::URtype::UR5: 27 | setTransZ(UR5_LINK_DIMENSIONS_d); 28 | setTransX(UR5_LINK_DIMENSIONS_a); 29 | break; 30 | case universalRobots::URtype::UR10: 31 | setTransZ(UR10_LINK_DIMENSIONS_d); 32 | setTransX(UR10_LINK_DIMENSIONS_a); 33 | break; 34 | default: 35 | setTransZ(UR10_LINK_DIMENSIONS_d); 36 | setTransX(UR10_LINK_DIMENSIONS_a); 37 | break; 38 | } 39 | 40 | m_d[m_numTransZ - 1] = endEffectorDimension; 41 | m_MDHmatrix << 0.0f, 0.0f, m_d[0], m_jointState[0].m_jointValue, // 0T1 42 | mathLib::rad(-90), 0.0f, m_d[1], m_jointState[1].m_jointValue + mathLib::rad(-90) ,// 1T2 43 | 0.0f, m_a[0], m_d[2], m_jointState[2].m_jointValue, // 2T3 44 | 0.0f, m_a[1], m_d[3], m_jointState[3].m_jointValue, // 3T4 45 | 0.0f, m_a[2], m_d[4], mathLib::rad(90) , // 4T4' 46 | mathLib::rad(90), 0.0f, 0.0f, m_jointState[4].m_jointValue, // 4'T5 47 | mathLib::rad(-90), 0.0f, 0.0f, mathLib::rad(-90) , // 5T5' 48 | 0.0f, m_a[3], m_d[5], m_jointState[5].m_jointValue, // 5'T6 49 | 0.0f, 0, m_d[6], 0 ; // 6T7 50 | } 51 | 52 | /// 53 | /// setRobotType 54 | /// 55 | /// 56 | void UR::setRobotType(const URtype& type) 57 | { 58 | m_type = type; 59 | } 60 | 61 | /// 62 | /// setTransZ 63 | /// 64 | /// 65 | void UR::setTransZ(const float(&d)[]) 66 | { 67 | memcpy(m_d, d, sizeof(m_d)); 68 | } 69 | 70 | /// 71 | /// setTransX 72 | /// 73 | /// 74 | void UR::setTransX(const float(&a)[]) 75 | { 76 | memcpy(m_a, a, sizeof(m_a)); 77 | } 78 | 79 | /// 80 | /// setMDHmatrix 81 | /// 82 | void UR::setMDHmatrix() 83 | { 84 | m_MDHmatrix << 0.0f, 0.0f, m_d[0], m_jointState[0].m_jointValue, // 0T1 85 | mathLib::rad(-90), 0.0f, m_d[1], m_jointState[1].m_jointValue + mathLib::rad(-90) ,// 1T2 86 | 0.0f, m_a[0], m_d[2], m_jointState[2].m_jointValue, // 2T3 87 | 0.0f, m_a[1], m_d[3], m_jointState[3].m_jointValue, // 3T4 88 | 0.0f, m_a[2], m_d[4], mathLib::rad(90) , // 4T4' 89 | mathLib::rad(90), 0.0f, 0.0f, m_jointState[4].m_jointValue, // 4'T5 90 | mathLib::rad(-90), 0.0f, 0.0f, mathLib::rad(-90) , // 5T5' 91 | 0.0f, m_a[3], m_d[5], m_jointState[5].m_jointValue, // 5'T6 92 | 0.0f, 0, m_d[6], 0 ; // 6T7 93 | } 94 | 95 | /// 96 | /// setTheta 97 | /// 98 | /// 99 | void UR::setTheta(const float(&jointVal)[]) 100 | { 101 | for (unsigned int i = 0; i < m_numDoF; i++) 102 | m_jointState[i].m_jointValue = jointVal[i]; 103 | } 104 | 105 | /// 106 | /// Returns the enum URtype. Used for printing purposes. 107 | /// 108 | /// m_type 109 | const URtype UR::getRobotType() const 110 | { 111 | return m_type; 112 | } 113 | 114 | /// 115 | /// Returns the values of the z-axis translations (d array). Used for printing purposes. 116 | /// 117 | /// m_d 118 | const float* UR::getTransZ() const 119 | { 120 | return m_d; 121 | } 122 | 123 | /// 124 | /// Returns the values of the x-axis translations (a array). Used for printing purposes. 125 | /// 126 | /// m_a 127 | const float* UR::getTransX() const 128 | { 129 | return m_a; 130 | } 131 | 132 | /// 133 | /// Returns the robot's current joint values. Used for printing purposes. 134 | /// 135 | /// m_theta 136 | const float UR::getTheta(const int &ix) const 137 | { 138 | return m_jointState[ix].m_jointValue; 139 | } 140 | 141 | /// 142 | /// Returns the robot's current tip pose. Used for printing purposes. 143 | /// 144 | /// m_tipPose 145 | const pose UR::getTipPose() const 146 | { 147 | return m_jointState[m_numDoF-1].m_jointPose; 148 | } 149 | 150 | /// 151 | /// Receives an array of target joint values and computes the pose of the robot's tip. 152 | /// 153 | /// 154 | /// tipPose 155 | pose UR::forwardKinematics(const float(&targetJointVal)[]) 156 | { 157 | // Assign joint values to compute MDH matrix. 158 | setTheta(targetJointVal); 159 | setMDHmatrix(); 160 | 161 | // Determine the indiviual transformation matrices. 162 | for (unsigned int i = 0; i < m_numReferenceFrames; i++) 163 | m_individualTransformationMatrices[i] = mathLib::calcTransformationMatrix(m_MDHmatrix.row(i)); 164 | 165 | // Determine the general transformation matrices. 166 | Eigen::Matrix3f rotationMatrix; 167 | unsigned int currentJoint = 0; 168 | pose tipPose = {}; 169 | for (unsigned int i = 0; i < m_numReferenceFrames; i++) 170 | { 171 | if(!i) 172 | m_generalTransformationMatrices[0] = m_individualTransformationMatrices[0]; 173 | else 174 | m_generalTransformationMatrices[i] = m_generalTransformationMatrices[i - 1] * m_individualTransformationMatrices[i]; 175 | 176 | 177 | // Obtaining the joint pose 178 | // Since we have more reference frames than joints only some represent a joint pose 179 | // 0T1 J1 // 1T2 J2// 2T3 J3// 3T4 J4// 4T4' XX// 4'T5 J5// 5T5' XX// 5'T6 J6// 6T7 tipPose 180 | if (i == 0 || i == 1 || i == 2 || i == 3 || i == 5 || i == 7 || i == 8) 181 | { 182 | // 0T1 1 // 1T2 2// 2T3 3// 3T4 4// 4T4' // 4'T5 5// 5T5' // 5'T6 6// 6T7 183 | rotationMatrix << m_generalTransformationMatrices[i](0, 0), m_generalTransformationMatrices[i](0, 1), m_generalTransformationMatrices[i](0, 2), 184 | m_generalTransformationMatrices[i](1, 0), m_generalTransformationMatrices[i](1, 1), m_generalTransformationMatrices[i](1, 2), 185 | m_generalTransformationMatrices[i](2, 0), m_generalTransformationMatrices[i](2, 1), m_generalTransformationMatrices[i](2, 2); 186 | 187 | float position[3] = { m_generalTransformationMatrices[i](0, 3), m_generalTransformationMatrices[i](1, 3), m_generalTransformationMatrices[i](2, 3) }; 188 | 189 | // Tip pose 190 | if (i == 8) 191 | tipPose = pose(position, rotationMatrix); 192 | else 193 | { 194 | m_jointState[currentJoint].m_jointPose = pose(position, rotationMatrix); 195 | currentJoint++; 196 | } 197 | } 198 | } 199 | 200 | //return m_jointState[m_numDoF-1].m_jointPose; 201 | return tipPose; 202 | } 203 | 204 | /// 205 | /// Computes the eight inverse kinematics solutions for a given tip pose. 206 | /// 207 | /// 208 | /// 209 | void UR::inverseKinematics(const pose &targetTipPose, float (*outIkSols)[m_numIkSol][m_numDoF]) 210 | { 211 | Eigen::Matrix4f T_07 = Eigen::Matrix4f::Identity(); // 0T7 212 | 213 | // Get translation matrix. 214 | const Eigen::Vector3f translation = { targetTipPose.m_pos[0], targetTipPose.m_pos[1], targetTipPose.m_pos[2] }; 215 | 216 | // Calculate rotation matrix. 217 | Eigen::Matrix3f rotation; 218 | rotation = Eigen::AngleAxisf(targetTipPose.m_eulerAngles[0], Eigen::Vector3f::UnitX()) 219 | *Eigen::AngleAxisf(targetTipPose.m_eulerAngles[1], Eigen::Vector3f::UnitY()) 220 | *Eigen::AngleAxisf(targetTipPose.m_eulerAngles[2], Eigen::Vector3f::UnitZ()); 221 | 222 | T_07.block<3, 3>(0, 0) = rotation; 223 | T_07.block<3, 1>(0, 3) = translation; 224 | 225 | // Computing theta1. 226 | const Eigen::Matrix P_05 = T_07 * Eigen::Matrix(0.0f, 0.0f, -m_d[5] - m_d[6], 1.0f).transpose(); // 0P5 position of reference frame {5} in relation to {0} 227 | const float theta1_psi = atan2(P_05(0, 1), P_05(0, 0)); 228 | 229 | // There are two possible solutions for theta1, that depend on whether the shoulder joint (joint 2) is left or right. 230 | const float theta1_phi = acos( (m_d[1] + m_d[2] + m_d[3] + m_d[4]) / (sqrt( pow(P_05(0, 1), 2) + pow(P_05(0, 0), 2) ) ) ); 231 | 232 | for (int i = -4; i < int (m_numIkSol) - 4 ; i++) 233 | { 234 | (*outIkSols)[i + 4][0] = (std::numbers::pi_v / 2 + theta1_psi + (i < 0 ? 1 : -1)* theta1_phi) - std::numbers::pi_v; // (i < 0 ? 1 : -1) first 4 theta1 values have positive phi 235 | } 236 | 237 | Eigen::Matrix4f T_06 = T_07; 238 | T_06(2, 3) = T_07(2, 3) - m_d[m_numTransZ - 1]; // 0T6 239 | 240 | for (unsigned int i = 0; i < m_numIkSol; i++) 241 | { 242 | // Computing theta5. 243 | Eigen::Matrix4f T_01 = mathLib::calcTransformationMatrix(Eigen::RowVector4f{ 0.0f, 0.0f, m_d[0], (*outIkSols)[i][0] }); // Knowing theta1 it is possible to know 0T1 244 | Eigen::Matrix4f T_16 = T_01.inverse() * T_06; // 1T6 = 1T0 * 0T6 245 | // There are two possible solutions for theta5, that depend on whether the wrist joint is up or down. 246 | if (i == 0 || i == 1 || i == 4 || i == 5) //(0, 1, 4, 5) 247 | (*outIkSols)[i][4] = acos( (T_16(1, 3) - (m_d[1] + m_d[2] + m_d[3] + m_d[4])) / m_d[5] ); 248 | else 249 | (*outIkSols)[i][4] = - acos( (T_16(1, 3) - (m_d[1] + m_d[2] + m_d[3] + m_d[4])) / m_d[5] ); 250 | 251 | // Computing theta6. 252 | if ((*outIkSols)[i][4] == 0 || (*outIkSols)[i][4] == 2 * std::numbers::pi_v) // If theta5 is equal to zero. 253 | (*outIkSols)[i][5] = 0.0f; // Give arbitrary value to theta6 254 | else 255 | { 256 | const float sinTheta5 = sin((*outIkSols)[i][4]); 257 | (*outIkSols)[i][5] = std::numbers::pi_v / 2 + atan2(-T_16.inverse()(1, 1) / sinTheta5, T_16.inverse()(0, 1) / sinTheta5); 258 | 259 | } 260 | 261 | 262 | // Computing theta3, theta2, and theta4. 263 | 264 | // T_45 = T_44'*T_4'5 265 | Eigen::Matrix4f T_44_ = mathLib::calcTransformationMatrix(Eigen::RowVector4f (0.0f, m_a[2], m_d[4], std::numbers::pi_v / 2)); 266 | Eigen::Matrix4f T_4_5 = mathLib::calcTransformationMatrix(Eigen::RowVector4f(std::numbers::pi_v / 2, 0.0f, 0.0f, (*outIkSols)[i][4])); 267 | Eigen::Matrix4f T_45 = T_44_ * T_4_5; 268 | 269 | // T_56 = T_55'*T_5'6 270 | Eigen::Matrix4f T_55_ = mathLib::calcTransformationMatrix(Eigen::RowVector4f(-std::numbers::pi_v / 2, 0.0f, 0.0f, -std::numbers::pi_v / 2)); 271 | Eigen::Matrix4f T_5_6 = mathLib::calcTransformationMatrix(Eigen::RowVector4f(0.0f, m_a[3], m_d[5], (*outIkSols)[i][5])); 272 | Eigen::Matrix4f T_56 = T_55_ * T_5_6; 273 | 274 | Eigen::Matrix4f T_46 = T_45 * T_56; 275 | Eigen::Matrix4f T_14 = T_16 * T_46.inverse(); 276 | 277 | float P_14_xz = sqrtf( pow(T_14(0, 3), 2) + pow(T_14(2, 3), 2)); 278 | float theta3_psi = acos((pow(P_14_xz, 2) - pow(m_a[1], 2) - pow(m_a[0], 2)) / (-2 * m_a[0] * m_a[1])); 279 | 280 | // Elbow up or down 281 | if ((i + 1) % 2 == 0) 282 | { 283 | // Computing theta3. 284 | (*outIkSols)[i][2] = std::numbers::pi_v - theta3_psi; 285 | // Masking theta3 for CoppeliaSim(invert value for ang > 180). 286 | if ((*outIkSols)[i][2] > std::numbers::pi_v) 287 | (*outIkSols)[i][2] = (*outIkSols)[i][2] - std::numbers::pi_v * 2; 288 | // Computing theta2. 289 | (*outIkSols)[i][1] = std::numbers::pi_v / 2 - atan2(T_14(2, 3), T_14(0, 3)) + asin((m_a[1] * sin(-theta3_psi)) / P_14_xz); 290 | // Computing theta4. 291 | Eigen::Matrix4f T_12 = mathLib::calcTransformationMatrix(Eigen::RowVector4f(-std::numbers::pi_v / 2, 0.0f, m_d[1], (*outIkSols)[i][1] - std::numbers::pi_v / 2)); 292 | Eigen::Matrix4f T_23 = mathLib::calcTransformationMatrix(Eigen::RowVector4f(0.0f, m_a[0], m_d[2], (*outIkSols)[i][2])); 293 | Eigen::Matrix4f T_03 = T_01 * T_12 * T_23; 294 | 295 | Eigen::Matrix4f T_36 = T_03.inverse() * T_06; 296 | Eigen::Matrix4f T_34 = T_36 * T_46.inverse(); 297 | 298 | (*outIkSols)[i][3] = atan2(T_34(1, 0), T_34(0, 0)); 299 | } 300 | else 301 | { 302 | // Computing theta3. 303 | (*outIkSols)[i][2] = std::numbers::pi_v + theta3_psi; 304 | // Masking theta3 for CoppeliaSim(invert value for ang > 180). 305 | if ((*outIkSols)[i][2] > std::numbers::pi_v) 306 | (*outIkSols)[i][2] = (*outIkSols)[i][2] - std::numbers::pi_v *2; 307 | // Computing theta2. 308 | (*outIkSols)[i][1] = std::numbers::pi_v / 2 - atan2(T_14(2, 3), T_14(0, 3)) + asin(m_a[1] * sin(theta3_psi) / P_14_xz); 309 | // Computing theta4. 310 | Eigen::Matrix4f T_12 = mathLib::calcTransformationMatrix(Eigen::RowVector4f(-std::numbers::pi_v / 2, 0.0f, m_d[1], (*outIkSols)[i][1] - std::numbers::pi_v / 2)); 311 | Eigen::Matrix4f T_23 = mathLib::calcTransformationMatrix(Eigen::RowVector4f(0.0f, m_a[0], m_d[2], (*outIkSols)[i][2])); 312 | Eigen::Matrix4f T_03 = T_01 * T_12 * T_23; 313 | 314 | Eigen::Matrix4f T_36 = T_03.inverse() * T_06; 315 | Eigen::Matrix4f T_34 = T_36 * T_46.inverse(); 316 | 317 | (*outIkSols)[i][3] = atan2(T_34(1, 0), T_34(0, 0)); 318 | } 319 | } 320 | } 321 | 322 | /// 323 | /// Generates a valid tip pose by running forward kinematics with random target joint values. 324 | /// 325 | /// randomValidTargetPose 326 | pose UR::generateRandomReachablePose() 327 | { 328 | //// https://en.cppreference.com/w/cpp/numeric/random/uniform_int_distribution 329 | std::random_device rd; 330 | std::mt19937 gen(rd()); 331 | std::uniform_int_distribution<> distrib(-360, 360); // URs joints limits [-360; 360] 332 | 333 | float randomTargetJointValue[m_numDoF] = {}; 334 | 335 | for (unsigned int i = 0; i < m_numDoF; i++) 336 | { 337 | randomTargetJointValue[i] = mathLib::rad(distrib(gen)); 338 | } 339 | 340 | return forwardKinematics(randomTargetJointValue); 341 | 342 | 343 | } 344 | 345 | 346 | /// 347 | /// Check the reachability of a target tip pose. 348 | /// 349 | /// 350 | /// bool 351 | bool UR::checkPoseReachability(const float(&ikSol)[]) const 352 | { 353 | for (unsigned int i = 0; i < 6; i++) 354 | { 355 | if(isnan(ikSol[i])) 356 | return false; 357 | } 358 | 359 | return true; 360 | } 361 | 362 | /// 363 | /// Operator overloading to be able to print a URtype enum. 364 | /// 365 | /// 366 | /// 367 | /// stream 368 | std::ostream& operator <<(std::ostream& stream, const universalRobots::URtype& type) 369 | { 370 | switch (type) 371 | { 372 | case universalRobots::URtype::UR3: 373 | stream << "UR3"; 374 | break; 375 | case universalRobots::URtype::UR5: 376 | stream << "UR5"; 377 | break; 378 | case universalRobots::URtype::UR10: 379 | stream << "UR10"; 380 | break; 381 | default: 382 | stream.setstate(std::ios_base::failbit); 383 | } 384 | return stream; 385 | } 386 | 387 | /// 388 | /// Operator overloading to be able to print a UR object. 389 | /// 390 | /// 391 | /// 392 | /// stream 393 | std::ostream& operator <<(std::ostream& stream, const universalRobots::UR& robot) 394 | { 395 | stream << "Robot type: " << robot.m_type << std::endl 396 | << "Number of DoFs: " << robot.m_numDoF << std::endl 397 | << "Link dimensions\n" << "Translations in the z-axis (meters):\n"; 398 | for (unsigned int i = 0; i < robot.m_numTransZ; i++) 399 | stream << "d" << i + 1 << ": " << robot.m_d[i] << std::endl; 400 | stream << "Translations in the x-axis (meters):\n"; 401 | for (unsigned int i = 0; i < robot.m_numTransX; i++) 402 | stream << "a" << i + 2 << ": " << robot.m_a[i] << std::endl; 403 | //stream << "Modified Denavit-Hartengerg Matrix\n"; 404 | //for (int x = 0; x < robot.m_numReferenceFrames; x++) // loop lines 405 | //{ 406 | // for (int y = 0; y < 4; y++) // loop columns 407 | // { 408 | // //stream << robot.getMDHmatrix()[x][y]; 409 | // stream << (robot.m_MDHmatrix[x][y]) << " "; 410 | // } 411 | // stream << endl; 412 | //} 413 | stream << "Joint values (degrees):\n"; 414 | for (unsigned int i = 0; i < robot.m_numDoF; i++) 415 | stream << "Theta" << i + 1 << ": " << mathLib::deg(robot.getTheta(i)) << std::endl; 416 | stream << "Tip pose:\n"; 417 | const pose tipPose = robot.getTipPose(); 418 | stream << "x " << tipPose.m_pos[0] << " y " << tipPose.m_pos[1] << " z " << tipPose.m_pos[2] << " (meters)\nalpha " 419 | << mathLib::deg(tipPose.m_eulerAngles[0]) << " beta " << mathLib::deg(tipPose.m_eulerAngles[1]) << " gamma " << mathLib::deg(tipPose.m_eulerAngles[2]) << " (degrees)" << std::endl; 420 | stream << "Individual Transformation Matrices:\n"; 421 | unsigned int counter = 0; 422 | for (unsigned int i = 0; i < robot.m_numReferenceFrames; i++) 423 | { 424 | if (i == 0 || i == 1 || i == 2 || i == 3 || i == 8) 425 | { 426 | stream << counter << "T" << counter + 1 << std::endl << robot.m_individualTransformationMatrices[i] << std::endl; 427 | counter++; 428 | } 429 | else 430 | { 431 | if(i == 4) 432 | stream << counter << "T" << counter << "'" << std::endl << robot.m_individualTransformationMatrices[i] << std::endl; 433 | if (i == 5) 434 | { 435 | stream << counter << "'T" << counter + 1 << std::endl << robot.m_individualTransformationMatrices[i] << std::endl; 436 | counter++; 437 | } 438 | if (i == 6) 439 | stream << counter << "T" << counter << "'" < 6 | #include 7 | #include "mathLib.h" 8 | 9 | #define UR3_LINK_DIMENSIONS_d { 0.1089f, 0.1115f, 0.0f, 0.0f, 0.0007f, 0.0818f, 0.0f } 10 | #define UR3_LINK_DIMENSIONS_a { 0.2437f, 0.2132f, 0.0842f, 0.0011f } 11 | 12 | #define UR5_LINK_DIMENSIONS_d { 0.0746f, 0.0703f, 0.0f, 0.0f, 0.0397f, 0.0829f, 0.0f } 13 | #define UR5_LINK_DIMENSIONS_a { 0.4251f, 0.3922f, 0.0456f, 0.0492f } 14 | 15 | #define UR10_LINK_DIMENSIONS_d { 0.109f, 0.10122f, 0.01945f, -0.00661f, 0.0584f, 0.09372f, 0.0f } 16 | #define UR10_LINK_DIMENSIONS_a { 0.6121f, 0.5722f, 0.0573f, 0.0584f } 17 | 18 | namespace universalRobots 19 | { 20 | 21 | /// URtype 22 | /// Different types of URs. 23 | /// 24 | enum URtype : unsigned int 25 | { 26 | UR3, UR5, UR10 // 0, 1, 2 27 | }; 28 | 29 | /// 30 | /// Structure which holds a pose { x y z } { alpha beta gamma } 31 | /// 32 | struct pose 33 | { 34 | float m_pos[3] = {}; // x y z (meters) 35 | float m_eulerAngles[3] = {}; // alpha beta gamma (radians) 36 | 37 | pose() 38 | : m_pos{ 0.0f, 0.0f, 0.0f }, m_eulerAngles{ 0.0f, 0.0f, 0.0f } {} 39 | 40 | pose(const float& pos1, const float& pos2, const float& pos3, const float& eulerAngles1, const float& eulerAngles2, const float& eulerAngles3) 41 | : m_pos{ pos1, pos2, pos3 }, m_eulerAngles{ eulerAngles1, eulerAngles2, eulerAngles3 } {} 42 | 43 | pose(const float(&pos)[], const float(&eulerAngles)[]) 44 | : m_pos{ pos[0], pos[1], pos[2] }, m_eulerAngles{ eulerAngles[0], eulerAngles[1], eulerAngles[2] } {} 45 | 46 | pose(const float(&pos)[], const Eigen::Matrix3f& rotationMatrix) 47 | : m_pos{ pos[0], pos[1], pos[2] }, m_eulerAngles{ rotationMatrix.eulerAngles(1, 2, 0).z(), rotationMatrix.eulerAngles(1, 2, 0).y(), rotationMatrix.eulerAngles(1, 2, 0).x() } {} 48 | 49 | pose divideByConst(const float& constant) const 50 | { 51 | return pose(m_pos[0]/ constant, m_pos[1] / constant, m_pos[2] / constant, m_eulerAngles[0] / constant, m_eulerAngles[1] / constant, m_eulerAngles[2] / constant ); 52 | } 53 | 54 | pose subtract(const pose& other) const 55 | { 56 | return pose(m_pos[0] - other.m_pos[0], m_pos[1] - other.m_pos[1], m_pos[2] - other.m_pos[2], m_eulerAngles[0] - other.m_eulerAngles[0], m_eulerAngles[1] - other.m_eulerAngles[1], m_eulerAngles[2] - other.m_eulerAngles[2]); 57 | } 58 | 59 | pose operator/(const float& constant) const 60 | { 61 | return divideByConst(constant); 62 | } 63 | 64 | pose operator-(const pose& other) const 65 | { 66 | return subtract(other); 67 | } 68 | 69 | }; 70 | 71 | /// 72 | /// Structure which holds the current value and pose of a joint. 73 | /// 74 | struct joint 75 | { 76 | pose m_jointPose = {}; 77 | float m_jointValue = 0.0f; 78 | 79 | joint() 80 | : m_jointPose(pose()), m_jointValue(0.0f) {} 81 | }; 82 | 83 | /// 84 | /// Implements 'UR robot type' object. 85 | /// 86 | class UR 87 | { 88 | public: 89 | 90 | /// 91 | /// Number of Degrees of Freedom is always 6 for all URs. 92 | /// 93 | static constexpr unsigned int m_numDoF = 6; 94 | 95 | /// 96 | /// Number of translations in the z-axis is always 7 (this includes a translation for the end-effector). 97 | /// 98 | static constexpr unsigned int m_numTransZ = 7; 99 | 100 | /// 101 | /// Number of translations in the z-axis is always 4. 102 | /// 103 | static constexpr unsigned int m_numTransX = 4; 104 | 105 | /// 106 | /// Number of frames is pre-defined (according to the MDH convention it is 9). 107 | /// 108 | static constexpr unsigned int m_numReferenceFrames = 9; 109 | 110 | /// 111 | /// Number of inverse kinematics solutions for a UR is 8. 112 | /// 113 | static constexpr unsigned int m_numIkSol = 8; 114 | 115 | /// 116 | /// This boolean indicates whether a tool is/isnt attached to the robot. 117 | /// Must be specified in the constructor, if not default is false. 118 | /// 119 | bool m_endEffector = false; 120 | 121 | /// 122 | /// Modified Denavit-Hartenberg parameters matrix (9x4) 123 | /// { alpha_i-1, a_i-1, d_i, theta_i } for each frame. 124 | /// 125 | Eigen::Matrix m_MDHmatrix { {0, 0, m_d[0], m_jointState[0].m_jointValue}, 126 | {mathLib::rad(-90), 0, m_d[1], m_jointState[1].m_jointValue + mathLib::rad(-90)}, 127 | {0, m_a[0], m_d[2], m_jointState[2].m_jointValue}, 128 | {0, m_a[1], m_d[3], m_jointState[3].m_jointValue}, 129 | {0, m_a[2], m_d[4], mathLib::rad(90)}, 130 | {mathLib::rad(90), 0, 0, m_jointState[4].m_jointValue}, 131 | {mathLib::rad(-90), 0, 0, mathLib::rad(-90)}, 132 | {0, m_a[3], m_d[5], m_jointState[5].m_jointValue}, 133 | {0, 0, m_d[6], 0} }; 134 | private: 135 | 136 | /// 137 | /// Robot type/name UR 3, 5, or 10. 138 | /// 139 | /// 0, 1, 2 140 | URtype m_type = UR10; 141 | 142 | /// 143 | /// d - translation (meters) in the z-axis (the last translation d[6] is for an end-effector). 144 | /// d_i is a nomeclature convention. 145 | /// 146 | float m_d[m_numTransZ] = UR10_LINK_DIMENSIONS_d; 147 | 148 | /// 149 | /// a - translation (meters) in the x-axis. 150 | /// a_i is a nomeclature convention. 151 | /// 152 | float m_a[m_numTransX] = UR10_LINK_DIMENSIONS_a; 153 | 154 | /// 155 | /// Position, Euler Angles, and Value of the robot's joints. {x, y, z} {alpha, beta, gamma} {jointValue} 156 | /// 157 | joint m_jointState[m_numDoF] = {}; 158 | 159 | // Create an array of (individual) transformation matrices iTi+1 / i-1Ti 160 | 161 | /// 162 | /// Array of (individual) transformation matrices iTi+1 / i-1Ti 163 | /// 164 | Eigen::Matrix4f m_individualTransformationMatrices[m_numReferenceFrames] = {}; 165 | 166 | /// 167 | /// Array of (general) transformation matrices 0Ti 168 | /// 169 | Eigen::Matrix4f m_generalTransformationMatrices[m_numReferenceFrames] = {}; 170 | 171 | public: 172 | UR(const URtype& robotType = UR10, const bool& endEffector = false, const float& endEffectorDimension = 0.0f); 173 | const URtype getRobotType() const; 174 | pose forwardKinematics(const float (&targetJointVal)[]); 175 | void inverseKinematics(const pose& targetTipPose, float(*outIkSols)[m_numIkSol][m_numDoF]); 176 | pose generateRandomReachablePose(); 177 | bool checkPoseReachability(const float(&targetTipPose)[]) const; 178 | friend std::ostream& operator <<(std::ostream& stream, const universalRobots::UR& robot); 179 | friend std::ostream& operator <<(std::ostream& stream, const universalRobots::URtype& type); 180 | private: 181 | void setMDHmatrix(); 182 | void setRobotType(const URtype& type); 183 | void setTransZ(const float (&d)[]); 184 | void setTransX(const float (&a)[]); 185 | void setTheta(const float (&jointVal)[]); 186 | const float* getTransZ() const; 187 | const float* getTransX() const; 188 | const float getTheta(const int& ix) const; 189 | const pose getTipPose() const; 190 | }; 191 | 192 | std::ostream& operator <<(std::ostream& stream, const universalRobots::URtype& type); 193 | 194 | std::ostream& operator <<(std::ostream& stream, const universalRobots::UR& robot); 195 | 196 | } // namespace universalRobots -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/universalRobotsKinematics/universalRobotsKinematics.vcxproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | Debug 14 | x64 15 | 16 | 17 | Release 18 | x64 19 | 20 | 21 | 22 | 16.0 23 | Win32Proj 24 | {7cd73ecd-a445-43e0-a840-adc6d9d2437e} 25 | universalRobotsKinematics 26 | 10.0 27 | 28 | 29 | 30 | StaticLibrary 31 | true 32 | v142 33 | Unicode 34 | 35 | 36 | StaticLibrary 37 | false 38 | v142 39 | true 40 | Unicode 41 | 42 | 43 | StaticLibrary 44 | true 45 | v142 46 | Unicode 47 | 48 | 49 | StaticLibrary 50 | false 51 | v142 52 | true 53 | Unicode 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | true 75 | $(SolutionDir)bin\$(Platform)\$(Configuration)\ 76 | $(SolutionDir)bin\inter\$(Platform)\$(Configuration)\ 77 | 78 | 79 | false 80 | $(SolutionDir)bin\$(Platform)\$(Configuration)\ 81 | $(SolutionDir)bin\inter\$(Platform)\$(Configuration)\ 82 | 83 | 84 | true 85 | $(SolutionDir)bin\$(Platform)\$(Configuration)\ 86 | $(SolutionDir)bin\inter\$(Platform)\$(Configuration)\ 87 | 88 | 89 | false 90 | $(SolutionDir)bin\$(Platform)\$(Configuration)\ 91 | $(SolutionDir)bin\inter\$(Platform)\$(Configuration)\ 92 | 93 | 94 | 95 | Level3 96 | true 97 | WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) 98 | true 99 | stdcpp20 100 | C:\Work\Eigen;$(SolutionDir)mathLib\src 101 | 102 | 103 | Console 104 | true 105 | 106 | 107 | 108 | 109 | Level3 110 | true 111 | true 112 | true 113 | WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 114 | true 115 | stdcpp20 116 | C:\Work\Eigen;$(SolutionDir)mathLib\src 117 | 118 | 119 | Console 120 | true 121 | true 122 | true 123 | 124 | 125 | 126 | 127 | Level3 128 | true 129 | _DEBUG;_CONSOLE;%(PreprocessorDefinitions) 130 | true 131 | stdcpp20 132 | C:\Work\Eigen;$(SolutionDir)mathLib\src 133 | 134 | 135 | Console 136 | true 137 | 138 | 139 | 140 | 141 | Level3 142 | true 143 | true 144 | true 145 | NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 146 | true 147 | stdcpp20 148 | C:\Work\Eigen;$(SolutionDir)mathLib\src 149 | 150 | 151 | Console 152 | true 153 | true 154 | true 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | {f9bbb153-b0c4-4dcb-940e-da420bed3376} 166 | 167 | 168 | 169 | 170 | 171 | -------------------------------------------------------------------------------- /cpp/universalRobotsKinematics/universalRobotsKinematics/universalRobotsKinematics.vcxproj.filters: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | Source Files 20 | 21 | 22 | 23 | 24 | Header Files 25 | 26 | 27 | --------------------------------------------------------------------------------