Inverse Dynamics Analysis of 6-dof Robotic Manipulator (UR5) by Forward & Backward Recursion RAJUL KUMAR Mobile Robotics R&D (Control & Dynamics) Addverb Technologies Pvt. Ltd Delhi Technological University Electronic copy available at: https://ssrn.com/abstract=3939013 ABSTRACT The project work performs the inverse dynamical analysis of 6-dof robotic manipulator (UR5) by performing newton Euler’s forward and backward recursion. Also, it can be termed as model-based torque control or computed torque control derivation. The simulation is performed in 3-D environment Gazebo simulator for unique robot description file of universal robot 5 (UR5). The resultant torques computed from dynamical equations for given angular configuration are directly fed into UR5 gazebo model joint effort topics. The simulation results and code execution give satisfactory results for dynamics derivation. Keywords: 6-dof robotic manipulator, Dynamics, recursion, gazebo Electronic copy available at: https://ssrn.com/abstract=3939013 1. Introduction Globally, a large population is suffering from motor disabilities caused by acute lesions to brain nervous system. Timely rehabilitation training is an efficient way to treat the stroke patients. Traditional manual therapy usually requires cooperative and intensive efforts from therapists and patients over prolonged sessions. Also, shortage of therapists, the expensive cost of rehabilitation training makes these practices difficult over years. To overcome these difficulties robot assisted medical techniques have been came into practice. But these applications require accurate mathematical modelling and dynamics of robotic manipulator. Fig 1,2. Robotic arms assisted surgery Electronic copy available at: https://ssrn.com/abstract=3939013 Dynamics of a system measure of exact forces & torques required to moves a manipulator link from a point to another giving smooth motion. The importance of dynamic analyses in robotics is that major control strategies are based on the dynamic models and feedbacks such as force, torque & impedance control. So, the more precise model and feedback can give birth to the more accurate control, which is very important for industrial applications such as pick & place, coring, drilling, grasping, scooping etc. The robotic arm consists of six revolute joints i.e., referred Base, Shoulder, Elbow, Wrist1, Wrist2 and Wrist3. Fig 3. Robotic arm measurements and workspace description The Shoulder and Elbow joint are rotating perpendicular to the Base joint. These three joints are connected with long links. In this way the robot has a long reach. The Wrist joints are mainly to man oeuvre the end-effector in the right orientation. Also, the UR5 is not confined to a box space but can reach far further. The green area is the area where the end effector can reach. Electronic copy available at: https://ssrn.com/abstract=3939013 The outer diameter of the sphere is 1.7 m and the diameter of the unreachable cylinder is 0.3 m. To simulate the robotic arm gazebo simulator is used with help of ROS in Ubuntu. The URDF.launch file code is stated below and the entire Unique robot description file is given in Appendix. Gazebo Launch File Code for URDF of 6-Dof Robotic Manipulator #Code Author – Rajul kumar <?xml version="1.0"?> <launch> <arg name="limited" default="false" doc="If true, limits joint range [-PI, PI] on all joints." /> <arg name="paused" default="false" doc="Starts gazebo in paused mode" /> <arg name="gui" default="true" doc="Starts gazebo gui" /> <!-- startup simulated world --> <include file="$(find gazebo_ros)/launch/empty_world.launch"> <arg name="world_name" default="worlds/empty.world"/> <arg name="paused" value="$(arg paused)"/> <arg name="gui" value="$(arg gui)"/> </include> <!-- send robot urdf to param server --> <include file="$(find ur_description)/launch/ur5_upload.launch"> <arg name="limited" value="$(arg limited)"/> </include> <!-- push robot_description to factory and spawn robot in gazebo --> <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -z 0.1" respawn="false" output="screen" /> <include file="$(find ur_gazebo)/launch/controller_utils.launch"/> <!-- start this controller --> <!--rosparam file="$(find ur_gazebo)/controller/arm_controller_ur5.yaml" command="load"/> <node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn arm_controller" respawn="false" output="screen"/> Electronic copy available at: https://ssrn.com/abstract=3939013 <!- load other controllers --> <!--node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false" output="screen" args="load joint_group_position_controller" /> <!-Load joint controller configurations from YAML file to parameter server --> <rosparam file="$(find ur_gazebo)/controller/ur5_controlT.yaml" command="load"/> <!--load the controllers --> <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/ur" args="joint_state_controller joint1_effort_controller joint2_effort_controller joint3_effort_controller joint4_effort_controller joint5_effort_controller joint6_effort_controller"/> <!--convert joint states to TF transforms for rviz, etc --> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen"> <remap from="/joint_states" to="/ur/joint_states" /> </node> </launch> 2. Forward Kinematics (DH-Convention of 6-dof robotic manipulator). Calculation of position of end effector for given values of joint angles of robot manipulator link is called forward kinematics. For robotic manipulators having higher Dof & series parallel combinations of coordinate frames calculating forward kinematics by simple geometric method is bit harder and inaccurate method. So to resolve this obstacle we use DH-Convention method for calculation forward kinematics of 6-Dof robotic manipulator. Electronic copy available at: https://ssrn.com/abstract=3939013 Fig. 4 Forward to inverse kinematics representation In order to define a rigid motion six parameters are generally needed: three parameters to define the rotation and three parameters to define the translation motion. The DH convention reduces the number of parameters that are needed to define a homogeneous transformation from six to four by exploiting the common manipulator geometry and by making a clever choice of the origin and the coordinate axes. The commonly used DHconvention defines four parameters that describe how the reference frame of each link is attached to the robot manipulator. Starting with the inertial reference frame, one additional reference frame is assigned for every link of the manipulator. Electronic copy available at: https://ssrn.com/abstract=3939013 Fig. 5. DH representation of manipulator The four parameters θi, di, ai, αi defined for each link i transforms reference frame (i−1) to (i) using the four basic transformations. The DH matrix can be obtained by using standard DH formula is given by equation (1) 𝐴𝑖 = 𝑅𝑜𝑡𝑧,θi 𝑇𝑟𝑎𝑛𝑠𝑧,di 𝑇𝑟𝑎𝑛𝑠𝑧,ai 𝑅𝑜𝑡𝑥,αi (1) where the matrices Rot describes rotation and the matrices Trans describes translation around the respective frames. The DH convention is a standard procedure used in the derivation of the forward kinematics of industrial manipulators. Electronic copy available at: https://ssrn.com/abstract=3939013 Table. 1. Resulting DH parameters of manipulator S no. θi di ai αi 1. θ1 d1 0 π/2 2. θ2 0 -a2 0 3. θ3 0 -a3 0 4. θ4 d4 0 π/2 5. θ5 d5 0 -π/2 6. θ6 d6 0 0 3. Dynamics derivation of 6-dof robotic manipulator The dynamics of a robot describe the relationship between forces, torques and motion. In mathematical terms, this relationship is presented using the equations of motion. We are intent to develop the complete dynamic model of the 6-dof Robotics manipulator (UR5). Several methods are used for deriving dynamics of robotic systems, some of them are - Lagrangian mechanics, Newton-Eulers mechanics. Generalized D’Alembert principle (G-D) - It derives the equations of motion in vector-matrix form suitable for control analysis. Electronic copy available at: https://ssrn.com/abstract=3939013 The G-D equations explicitly identify the contributions of the translational and rotational effects of the links. Bond Graph Technique – It is a graphical approach for diagramming the distribution and flow of power and energy within a dynamic system. Zero Reference Position Method - A suitable configuration of the manipulator is designated as the zero-reference position where all of the joint variables have zero values. Kane's method (also known as Lagrange's form of D'Alembert's Principle) - It is a vector formulation based on a modification of Newton's equations. and requires the computation of absolute velocity and acceleration. Fig. 6. Dynamic motion representation of manipulator Apart from these techniques most common methods are the Lagrangian and Newton-Euler method. Electronic copy available at: https://ssrn.com/abstract=3939013 The Lagrangian method is based on the difference between the kinetic energy K and the potential energy P of the manipulator. We have used Newton-Eulers method for dynamical modelling of UR5 because the equations of motion themselves are usually more complicated computationally when developed using Lagrangian mechanics. And the major drawback to this method was the high computational cost which limited the use of real-time inverse dynamics on manipulators. Also, in Lagrangian method treats the entire manipulator as one system but the recursive Newton-Euler method treats each link separately. Which gives more accurate motion of equations as every part of the robot id analyzed. Fig. 7. Libraries used for deriving dynamics For deriving equations several symbolic software's are used like MATLAB, maple, Mathematica. Electronic copy available at: https://ssrn.com/abstract=3939013 But due open-source restrictions, we have used Sympy Dynamics Physics Vector and NumPy python libraries. Newton-Euler method uses recursive formulation forces and torques between individual links to develop the dynamics. The complete dynamic model of the 6-dof UR5 manipulator is derived to equation (2) 𝜏 = 𝑀(𝜃)𝜃 + 𝐶(𝜃, 𝜃)𝜃 + g(𝜃) (2) In general, such a model can be used for at least two different purposes first in direct dynamics, where we are interested in the resulting state (q,q˙) of the robot given known input torques (τ). And another in inverse dynamics, where we want to find the input torques (τ) to control the robot to certain states (q, q˙) over time. Inverse dynamics is commonly used in controllers, such as the inverse dynamics controller which will be discuss in further sections. Step by step procedure I have followed for deriving inverse dynamics equations are given below The dynamics of a manipulator using the Newton-Euler method is developed in two parts. 1. The first part treats velocities and accelerations by making the same set of calculations for each link starting at the first. This is called the forward recursion. 2. The second part starts at the last link and treats forces and torques and how these propagate to the first link. This second part is called the backward recursion. Backward recursion completely depends upon forward recursion as the calculated angular accelerations and velocities has to be inserted into torque calculation formulas. The general requirements of System Parameters for Dynamical Modelling are stated below – Electronic copy available at: https://ssrn.com/abstract=3939013 1. Vector from origin of frame (i) to the center of mass of link (i) that is 𝑟𝑖,𝑐𝑖 . Basically, we need center of mass of each link to calculate this vector quantity as shown in figure 8. Fig. 8. Vector 𝑟𝑖,𝑐𝑖 representation 2. Vector from origin of frame (i-1) to the origin of link (i) that is 𝑟𝑖−1,𝑖 . And we need DH Parameters to calculate this vector quantity. Fig. 9. Vector 𝑟𝑖−1,𝑖 representation Electronic copy available at: https://ssrn.com/abstract=3939013 3. Mass and Inertia of each link. We can get these parameters from solid works or cad file. Fig. 10. Calculation of MI for each link 1 𝐼𝑥𝑥 = 12 𝑀(3𝑟2 + 𝑙2) 𝐼𝑥𝑦 = 𝐼𝑦𝑥 =0 (3) (4) 𝐼𝑦𝑦 = 12 𝑀(3𝑟2 + 𝑙2) 𝐼𝑥𝑧 = 𝐼𝑧𝑥 =0 𝐼𝑦𝑧 = 𝐼𝑧𝑦 =0 1 2 𝐼 𝑧𝑧 = 𝑀𝑟 5 (5) (6) (7) (8) 1 4. After getting all the required parameters we can start forward recursion starting from the world base we assume that 𝜔𝑖, 𝛼𝑖, 𝑎𝑐,𝑖, 𝑎𝑒,𝑖 are zero for 𝑖 = 0. 𝜔0 =0 𝑎𝑐,0 =0 𝛼0 =0 𝑎𝑒,0 =0 Where 𝜔𝑖 & 𝛼𝑖 are angular velocity & acceleration of frame 𝑖 w.r.t. frame 0. And 𝑎𝑐,𝑖 & 𝑎𝑒,𝑖 are acceleration of the center of mass & end of the link 𝑖. Electronic copy available at: https://ssrn.com/abstract=3939013 5. By solving following equations in order of appearance from base to end effector we can get the all these required for inserting into backward recursion equations. 𝑖 𝜔 𝜔𝑖 = 𝑅𝑖−1 𝑖−1 + 𝑏𝑖𝜃𝑖 (9) 𝑖 Where 𝑅𝑖−1 are rotation matrix from frame 𝑖 + 1 to frame 𝑖. And 𝑏𝑖 is axis of rotation of joint 𝑖 expressed in frame 𝑖. 𝑖 𝛼𝑖 = 𝑅𝑖−1 𝛼𝑖−1 + 𝑏𝑖 θ𝑖 + 𝜔𝑖 × 𝑏𝑖 𝜃𝑖 (10) 𝑖 𝑎𝑒,𝑖 = 𝑅𝑖−1 𝑎𝑒,𝑖−1 + 𝜔𝑖 × 𝑟𝑖−1,𝑖 + 𝜔𝑖 × (𝜔𝑖 × 𝑟𝑖−1,𝑖 ) (11) 𝑖 𝑎𝑐,𝑖 = 𝑅𝑖−1 𝑎𝑒,𝑖−1 + 𝜔𝑖 × 𝑟𝑖−1,𝑐𝑖 + 𝜔𝑖 × (𝜔𝑖 × 𝑟𝑖−1,𝑐𝑖 ) 𝑖 𝑔 𝑔𝑖 = 𝑅𝑖−1 𝑖−1 (12) (13) Where 𝑔𝑖 gravity matrix of frame 𝑖 w.r.t previous frame i.e., 𝑖+1 frame. By putting the value of i from 1 to 6 we can drive these equations for 6 links of the robotic arm. After getting theses equations we reached at end effector of the manipulator and the forward recursion ends. After reaching at end effector by backward recursion we start our forward recursion with terminal conditions. 𝑓𝑛+1 =0 𝑟𝑛+1 =0 Here 𝑓𝑖 & 𝑟𝑖 are the force & torque exerted by link 𝑖 + 1 on link 𝑖. 𝑖 𝑓𝑖 = 𝑅𝑖+1 𝑓𝑖+1 + 𝑚𝑖𝑎𝑐,𝑖-𝑚𝑖𝑔𝑖 𝑟𝑖 = 𝑅𝑖 𝑟𝑖+1 − 𝑓𝑖 × 𝑟𝑖−1,𝑐𝑖 + (𝑅𝑖 𝑖+1 𝑖+1 𝑓𝑖+1) × 𝑟𝑖,𝑐𝑖 + 𝐼𝑖 𝛼𝑖 + 𝜔𝑖 × 𝐼𝑖 𝜔𝑖 Electronic copy available at: https://ssrn.com/abstract=3939013 (14) (15) By putting the value of i from 6 to 1 we can drive force & torque equations for 6 links of the robotic arm. After getting theses equations we reached at the base of the manipulator with all the derived Dynamical equations of forces & torques. These equations can be used directly for inverse dynamics in which 𝜃𝑖, 𝜃𝑖 and θ𝑖 is provided and the corresponding 𝑟𝑖 is found. However, for simulation purposes and implementation of PID controllers with inertia matrix the Dynamical equations needs to be in a form of M(𝜃𝑖), C(𝜃𝑖, 𝜃𝑖 ), and g(𝜃𝑖). Where M(𝜃𝑖) is inertia matrix, C(𝜃𝑖, 𝜃𝑖 ) is Coriolis and centripetal acceleration matrix & g(𝜃𝑖) is the gravity matrix of the manipulator. The python code for dynamics is given below for universal robot 5 model. # -*- coding: utf-8 -*""" Created on Tue May 17 06:54:18 2021 @author: rajul Kumar """ from sympy.physics.vector import dynamicsymbols from sympy import diff, Symbol from sympy.physics.vector import * from sympy import * import numpy as np import pandas as pd q1,q2,q3,q4,q5,q6 = dynamicsymbols('q1 q2 q3 q4 q5 q6') q = Matrix([q1,q2,q3,q4,q5,q6]) #* (180/(np.pi dq = diff(q,Symbol('t')) ddq = diff(dq,Symbol('t')) r1c1 r2c2 r3c3 r4c4 r5c5 r6c6 r01 r12 r23 r34 r45 r56 = = = = = = = = = = = = np.array([0, -0.02561, 0.00193]) np.array([0.2125, 0, 0.11336]) np.array([0.11993, 0, 0.0265]) np.array([0, -0.0018, 0.01634]) np.array([0, 0.0018, 0.01634]) np.array([0, 0, -0.001159]) np.array([0, 0.089159, 0]) np.array([0.42500, 0, 0]) np.array([0.39225,0,0]) np.array([0, 0.10915, 0]) np.array([0, 0.09465, 0]) np.array([0, 0, 0.0823]) Electronic copy available at: https://ssrn.com/abstract=3939013 r0c1 r1c2 r2c3 r3c4 r4c5 r5c6 = = = = = = r1c1 r2c2 r3c3 r4c4 r5c5 r6c6 + + + + + + r01 r12 r23 r34 r45 r56 g0 = np.array([0,0,-9.806]) alp1 alp2 alp3 alp4 alp5 alp6 = = = = = = 1.5707 0 0 1.5707 -1.5707 0 alp = Matrix([alp1,alp2,alp3,alp4,alp5,alp6]) #* (180/(np.pi)) for i in range(0,6): rzq = Matrix([[cos(q[i]),sin(q[i]),0],[sin(q[i]),cos(q[i]),0],[0,0,1]]) rxalp = Matrix([[1,0,0],[0,cos(alp[i]),sin(alp[i])],[0,sin(alp[i]),cos(alp[i])]]) if i == 0: R01 = np.matmul(rzq,rxalp) elif i == 1: R12 = np.matmul(rzq,rxalp) elif i == 2: R23 = np.matmul(rzq,rxalp) elif i == 3: R34 = np.matmul(rzq,rxalp) elif i == 4: R45 = np.matmul(rzq,rxalp) else: R56 = np.matmul(rzq,rxalp) R02 R03 R04 R05 R06 = = = = = np.matmul(R01,R12) np.matmul(R02,R23) np.matmul(R03,R34) np.matmul(R04,R45) np.matmul(R05,R56) z0 = np.array([0,0,1]) b1 b2 b3 b4 b5 b6 = = = = = = np.matmul((R01.T),z0) np.matmul((R02.T),np.matmul(R01,z0)) np.matmul((R03.T),np.matmul(R02,z0)) np.matmul((R04.T),np.matmul(R03,z0)) np.matmul((R05.T),np.matmul(R04,z0)) np.matmul((R06.T),np.matmul(R05,z0)) i1 = Matrix([[0.016926823, 0, 0], [0, 0.016926823, 0], [0, 0, 0.00666]]) i2 = Matrix([[0.241988487, 0, 0],[0,0.241988487, 0],[0, 0, 0.0151074]]) i3 = Matrix([[0.035310391, 0, 0],[0, 0.035310391, 0],[0, 0, 0.004095]]) i4 = Matrix([[0.00475391, 0, 0],[0, 0.00475391, 0],[0, 0, 0.0021942]]) i5 = Matrix([[0.00475391, 0, 0],[0, 0.00475391, 0],[0, 0, 0.0021942]]) i6 = Matrix([[0.000216804, 0, 0],[0, 0.000216804, 0],[0, 0, 0.000132117]]) Electronic copy available at: https://ssrn.com/abstract=3939013 w1 = (b1 * dq[0]) alpha1 = (b1 * ddq[0]) + np.cross(w1,(b1 * dq[0])) dw1 = diff(w1,Symbol('t')) ae1 = np.cross(dw1,r01) + np.cross(w1,np.cross(w1,r01)) ac1 = np.cross(dw1,r0c1) + np.cross(w1,np.cross(w1,r0c1)) g1 = np.matmul((R01.T),g0) w2 = np.matmul((R12.T),w1) + (b2 * dq[1]) alpha2 = np.matmul((R12.T),alpha1) + (b2 * ddq[1]) + np.cross(w2,(b2 * dq[1])) dw2 = diff(w2,Symbol('t')) ae2 = np.matmul((R12.T),ae1) + np.cross(dw2,r12) + np.cross(w2,np.cross(w2,r12)) ac2 = np.matmul((R12.T),ae1) + np.cross(dw2,r1c2) + np.cross(w2,np.cross(w2,r1c2)) g2 = np.matmul((R12.T),g1) w3 = np.matmul((R23.T),w2) + (b3 * dq[2]) alpha3 = np.matmul((R23.T),alpha2) + (b3 * ddq[2]) + np.cross(w3,(b3 * dq[2])) dw3 = diff(w3,Symbol('t')) ae3 = np.matmul((R23.T),ae2) + np.cross(dw3,r23) + np.cross(w3,np.cross(w3,r23)) ac3 = np.matmul((R23.T),ae2) + np.cross(dw3,r2c3) + np.cross(w3,np.cross(w3,r2c3)) g3 = np.matmul((R23.T),g2) w4 = np.matmul((R34.T),w3) + (b4 * dq[3]) alpha4 = np.matmul((R34.T),alpha3) + (b4 * ddq[3]) + np.cross(w4,(b4 * dq[3])) dw4 = diff(w4,Symbol('t')) ae4 = np.matmul((R34.T),ae3) + np.cross(dw4,r34) + np.cross(w4,np.cross(w4,r34)) ac4 = np.matmul((R34.T),ae3) + np.cross(dw4,r3c4) + np.cross(w4,np.cross(w4,r3c4)) g4 = np.matmul((R34.T),g3) w5 = np.matmul((R45.T),w4) + (b5 * dq[4]) alpha5 = np.matmul((R45.T),alpha4) + (b5 * ddq[4]) + np.cross(w5,(b5 * dq[4 dw5 = diff(w5,Symbol('t')) ae5 = np.matmul((R45.T),ae4) + np.cross(dw5,r45) + np.cross(w5,np.cross(w5,r45)) ac5 = np.matmul((R45.T),ae4) + np.cross(dw5,r4c5) + np.cross(w5,np.cross(w5,r4c5)) g5 = np.matmul((R45.T),g4) w6 = np.matmul((R56.T),w5) + (b6 * dq[5]) alpha6 = np.matmul((R56.T),alpha5) + (b6 * ddq[5]) + np.cross(w6,(b6 * dq[5 dw6 = diff(w6,Symbol('t')) ae6 = np.matmul((R56.T),ae5) + np.cross(dw6,r56) + np.cross(w6,np.cross(w6,r56)) ac6 = np.matmul((R56.T),ae5) + np.cross(dw6,r5c6) + np.cross(w6,np.cross(w6,r5c6)) g6 = np.matmul((R56.T),g5) m1 m2 m3 m4 m5 m6 = = = = = = 3.7000 8.3930 2.2750 1.2190 1.2190 0.1879 Electronic copy available at: https://ssrn.com/abstract=3939013 f6 = (m6 * ac6) - (m6 * g6) t6 = -np.cross(f6,r5c6) + np.matmul(i6,alpha6) + np.cross(w6,(np.matmul(i6,w6))) tr6 = np.matmul((b6.T),t6) print(tr6) f5 = np.matmul(R56,f6) + (m5 * ac5) - (m5 * g5) t5 = np.matmul(R56,t6) - np.cross(f5,r4c5) + np.cross((np.matmul(R56,f6)),r5c5) + np.matmul(i5,alpha5) + np.cross(w5,(np.matmul(i5,w5))) tr5 = np.matmul((b5.T),t5) print(tr5) f4 = np.matmul(R45,f5) + ((m4 * ac4) - (m4 * g4)) t4 = np.matmul(R45,t5) - np.cross(f4,r3c4) + np.cross((np.matmul(R45,f5)),r4c4) +np.matmul(i4,alpha4) + np.cross(w4,(np.matmul(i4,w4))) tr4 = np.matmul((b4.T),t4) print(tr4) f3 = np.matmul(R34,f4) + ((m3 * ac3) - (m3 * g3)) t3 = np.matmul(R34,t4) - np.cross(f3,r2c3) + np.cross((np.matmul(R34,f4)),r3c3) + np.matmul(i3,alpha3) + np.cross(w3,(np.matmul(i3,w3))) tr3 = np.matmul((b3.T),t3) print(tr3) f2 = np.matmul(R23,f3) + ((m2 * ac2) - (m2 * g2)) t2 = np.matmul(R23,t3) - np.cross(f2,r1c2) + np.cross((np.matmul(R23,f3)),r2c2) +np.matmul(i2,alpha2) + np.cross(w2,(np.matmul(i2,w2))) tr2 = np.matmul((b2.T),t2) print(tr2) f1 = np.matmul(R12,f2) + ((m1 * ac1) - (m1 * g1)) t1 = np.matmul(R12,t2) - np.cross(f1,r0c1) + np.cross((np.matmul(R12,f2)),r1c1) +np.matmul(i1,alpha1) + np.cross(w1,(np.matmul(i1,w1))) tr1 = np.matmul((b1.T),t1) print(tr1) ********************************************************** 4 . Results & Simulation 1. Test Case 1: 𝜃1= 0𝑜, 𝜃2= 90𝑜, 𝜃3=𝜃3∗ , 𝜃4= 0𝑜, 𝜃5= 0𝑜, 𝜃6= 0𝑜. The link-2 is kept at 90𝑜 and all other links are kept at 0𝑜. And link-3 (𝜃3∗) is varying from 0𝑜 to 360𝑜 at a difference of 1𝑜. Torque on link3 (𝑟3) Vs. Variable position (𝜃3∗) is shown in graph. The resultant graph is coming as expected a sine curve. Which validates that torque3 equation are doing recursion smoothly and showing correct results. Electronic copy available at: https://ssrn.com/abstract=3939013 Fig. 11. Test Case 1 configuration The resultant graph is coming as expected a sine curve. Which validates that torque-3 equation are doing recursion smoothly and showing correct results. 2. Test Case 2: 𝜃1= 0𝑜, 𝜃2=𝜃2∗, 𝜃3=0𝑜, 𝜃4= 0𝑜, 𝜃5= 0𝑜, 𝜃6= 0𝑜 All the other links are kept at 0𝑜. And link-2 (𝜃2∗) is varying from 0𝑜 to 360𝑜 at a difference of 1𝑜. Torque on link-2 (𝑟2) Vs. Variable position (𝜃2∗) is shown in graph. Fig. 12. Test Case 2 configuration Electronic copy available at: https://ssrn.com/abstract=3939013 The resultant graph is coming as expected. Which validates that torqu2 equations are doing recursion smoothly and showing correct results. 3. Test Case 3: 𝜃1= 0𝑜, 𝜃2=𝜃2∗, 𝜃3=90𝑜, 𝜃4= 0𝑜, 𝜃5= 0𝑜, 𝜃6= 0𝑜 The link-3 is kept at 90𝑜 and all other links are kept at 0𝑜. And link-2 (𝜃3∗) is now moving from 0𝑜 to 360𝑜 at a difference of 1𝑜. Torque on link-2 (𝑟2) Vs. Variable position (𝜃∗2) is shown in graph. Fig. 13. Test Case 3 configuration The resultant graph is coming as expected. From previous configuration it takes less torque to rotate arm. 4. Test Case 4: 𝜃1=𝜃1∗, 𝜃2=0𝑜, 𝜃3=0𝑜, 𝜃4=0𝑜, 𝜃5=0𝑜, 𝜃6= 0𝑜 All other links are kept at 0𝑜. And link-1 (𝜃1∗) is varying from 0𝑜 to 360𝑜 at a difference of 1𝑜. Torque on link-1 (𝑟1) Vs. Variable position (𝜃1∗) is shown in graph. Electronic copy available at: https://ssrn.com/abstract=3939013 Fig. 14. Test Case 4 configuration The resultant graph is coming as expected. Torque should be 0 & constant at every position of link 1. 5. Test Case 5: 𝜃1= 0𝑜, 𝜃2= 90𝑜, 𝜃3=90𝑜, 𝜃4=𝜃4∗, 𝜃5= 0𝑜, 𝜃6= 0𝑜 The link-2 & 3 is kept at 90𝑜 and all other links are kept at 0𝑜. And link-4 (𝜃4∗) is varying from 0𝑜 to 360𝑜 at a difference of 1𝑜. Torque on link-4 (𝑟4) Vs. Variable position (𝜃4∗) is shown in graph. The resultant graph is coming as expected a sine curve. Which validates that torque-4 equation are doing recursion smoothly and showing correct results. Electronic copy available at: https://ssrn.com/abstract=3939013 Fig. 15. Test Case 5 configuration 6. Test Case 6: 𝜃1= 0𝑜, 𝜃2=90𝑜, 𝜃3=0𝑜, 𝜃4= 0𝑜, 𝜃5=𝜃5∗, 𝜃6= 0𝑜 The link-2 is kept at 90𝑜 and all other links are kept at 0𝑜. And link-5 (𝜃5∗) is varying from 0𝑜 to 360𝑜 at a difference of 1𝑜. Torque on link5 (𝑟5) Vs. Variable position (𝜃5∗) is shown in graph. The resultant graph is coming as expected. Which validates that torque-5 equation are doing recursion smoothly and showing correct results. Fig. 16. Test Case 6 configuration 7.Test Case 7: 𝜃1= 0𝑜, 𝜃2=90𝑜, 𝜃3=90𝑜, 𝜃4=90𝑜, 𝜃5=𝜃∗5, 𝜃6= 0𝑜 The link-2, 3 & 4 is kept at 90𝑜 and all other links are kept at 0𝑜. And link-5 (𝜃5∗) is varying from 0𝑜 to 360𝑜 at a difference of 1𝑜. Torque on link-5 (𝑟5) Vs. Variable position (𝜃5∗) is shown in graph. The resultant graph is coming as expected. The mass of link-5 is less so it required low value of torque. Electronic copy available at: https://ssrn.com/abstract=3939013 Fig. 17. Test Case 7 configuration 8. Test Case 8: 𝜃1= 0𝑜, 𝜃2=90𝑜, 𝜃3=90𝑜, 𝜃4=90𝑜, 𝜃5=90𝑜, 𝜃6=𝜃∗6 The link-2, 3, 4 & 5 is kept at 90𝑜 and all other links are kept at 0𝑜. And link-6 (𝜃6∗) is varying from 0𝑜 to 360𝑜 at a difference of 1𝑜. Torque on link-6 (𝑟6) Vs. Variable position (𝜃6∗) is shown in graph. The resultant graph is coming as expected. The mass of link-6 is less so it required low value of torque. Fig. 18. Test Case 8 configuration Electronic copy available at: https://ssrn.com/abstract=3939013 9. Final Test Case 9: 𝜃1=0, 𝜃2=0.9, 𝜃3= -1.0, 𝜃4= 1.0, 𝜃5= 3.5, 𝜃6= -1.9 (Radians) The Torque values calculated from dynamical equations are fed into gazebo each joint effort topics for a random configuration. Torque For now Position constraints are ignored and standing or moving of UR5 in gazebo model is considered as the evaluation criteria for the dynamical equations. As simulation result UR5 moved & aliened itself in a random configuration. Electronic copy available at: https://ssrn.com/abstract=3939013 5. Future Work In the previous sections, the dynamics of the UR5 manipulator was developed. In order to control the manipulator to a desired trajectory, a controller 𝐾 is needed. This controller takes a smooth desired trajectory and returns the motor torque to follow it. 𝑟 = 𝑀(𝜃)𝜃 + 𝐶(𝜃, 𝜃)𝜃 + g(𝜃) (16) 𝑟 = 𝑀(𝜃)𝐾 + 𝐶(𝜃, 𝜃)𝜃 + g(𝜃) (17) 𝑀(𝜃)𝜃 + 𝐶(𝜃, 𝜃)𝜃 + g(𝜃) = 𝑀(𝜃)𝐾 + 𝐶(𝜃, 𝜃)𝜃 + g(𝜃) (18) 𝑀(𝜃)𝜃 = 𝑀(𝜃)𝐾 (19) 𝜃 =𝐾 (20) This is known as a double-integrator system, which essentially let us control the joint acceleration independently of other joints. (The Control equation 𝐾 is feedforward control with a PID feedback.) 𝜃𝑒 = 𝜃𝑑 − 𝜃 (21) 𝑡 𝐾 =𝜃 −𝐾 𝜃 −𝐾 ∫ 𝜃 𝑝 𝑒 𝑖 0 𝑒 −𝐾 𝑑𝜃𝑒 𝑑 𝑑𝑡 (22) Where 𝜃𝑒 is error in joint angle, 𝜃𝑑 is desired joint angle & 𝜃 is current joint angle position. By choosing the appropriate gain matrices 𝐾𝑝, 𝐾𝑖 & 𝐾𝑑 . 𝑡 𝐾 =𝜃 −𝐾 𝜃 −𝐾 ∫ 𝜃 𝑝 𝑒 𝑖 0 𝑒 −𝐾 𝑑𝜃𝑒 𝑑 𝑑𝑡 Electronic copy available at: https://ssrn.com/abstract=3939013 (23) 𝐾𝑝1 ⋯ 𝐾𝑝6 ⋱ ⋮ ] 𝐾𝑝=[ ⋮ 𝐾𝑝6 ⋯ 𝐾𝑝6 (24) 𝐾𝑖1 ⋯ 𝐾𝑖6 ⋱ ⋮ ] 𝐾𝑖=[ ⋮ 𝐾𝑖6 ⋯ 𝐾𝑖6 (25) 𝐾𝑑1 ⋯ 𝐾𝑑6 ⋱ ⋮ ] 𝐾𝑑=[ ⋮ 𝐾𝑑6 ⋯ 𝐾𝑑6 (26) The gain matrices have been defined, we can find 𝐾 by providing a reference trajectory (𝜃𝑑 , 𝜃𝑑 , 𝜃𝑑 ). And by using the dynamics of the manipulator as found in the previous slides, we can now find the input torque 𝑟 using equation 𝑟 = 𝑀(𝜃)𝐾 + 𝐶(𝜃, 𝜃)𝜃 + g(𝜃) 𝐾𝑝1 ⋯ ⋱ 𝑟 = 𝑀(𝜃) ([ ⋮ 𝐾𝑝6 ⋯ 𝐾𝑝6 𝐾𝑖1 ⋯ 𝐾𝑖6 𝐾𝑑1 ⋯ 𝐾𝑑6 𝑑𝜃𝑒 𝑡 ⋱ ⋮ ] ∫ 𝜃𝑒 + [ ⋮ ⋱ ⋮ ] ) + 𝐶(𝜃, 𝜃)𝜃 + g(𝜃) ⋮ ] 𝜃𝑒 + [ ⋮ 0 𝐾𝑖6 ⋯ 𝐾𝑖6 𝐾𝑑6 ⋯ 𝐾𝑑6 𝑑𝑡 𝐾𝑝6 (27) (28) 5. Conclusion Dynamical model of UR5 6-dof robotic manipulator is derived using Newton-Eulers method. For symbolic calculations Sympy & Numpy in python is used. The derived dynamical torque equations are tested for various configurations. The resultant plots of torque vs angular position are giving satisfied curves. Which validates the dynamic equations. Moreover, simulation of UR5 for standing torque is done at random configuration of gazebo. After publishing torque values calculated from dynamical equations the UR5 aliened itself at random position as required. Electronic copy available at: https://ssrn.com/abstract=3939013 Appendix Unique Robot Description File of 6-dof robotic manipulator used in project work (Universal Robot 5) <?xml version="1.0"?> <robot xmlns:xacro="http://wiki.ros.org/xacro"> <xacro:include filename="$(find ur_description)/urdf/ur.transmission.xacro" /> <xacro:include filename="$(find ur_description)/urdf/ur.gazebo.xacro" /> <xacro:macro name="cylinder_inertial" params="radius length mass *origin"> <inertial> <mass value="${mass}" /> <xacro:insert_block name="origin" /> <inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0" iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0" izz="${0.5 * mass * radius * radius}" /> </inertial> </xacro:macro> <xacro:macro name="ur5_robot" params="prefix joint_limited shoulder_pan_lower_limit:=${-pi} shoulder_pan_upper_limit:=${pi} shoulder_lift_lower_limit:=${-pi} shoulder_lift_upper_limit:=${pi} elbow_joint_lower_limit:=${-pi} elbow_joint_upper_limit:=${pi} wrist_1_lower_limit:=${-pi} wrist_1_upper_limit:=${pi} wrist_2_lower_limit:=${-pi} wrist_2_upper_limit:=${pi} wrist_3_lower_limit:=${-pi} wrist_3_upper_limit:=${pi} transmission_hw_interface:=hardware_interface/PositionJointInt erface safety_limits:=false safety_pos_margin:=0.15 safety_k_position:=20"> <!-- Inertia parameters --> <xacro:property name="base_mass" value="4.0" /> <!-- This mass might be incorrect --> <xacro:property name="shoulder_mass" value="3.7000" /> <xacro:property name="upper_arm_mass" value="8.3930" /> <xacro:property name="forearm_mass" value="2.2750" /> <xacro:property name="wrist_1_mass" value="1.2190" /> Electronic copy available at: https://ssrn.com/abstract=3939013 <xacro:property name="wrist_2_mass" value="1.2190" /> <xacro:property name="wrist_3_mass" value="0.1879" /> <xacro:property 0.02561" /> <xacro:property 0.2125" /> <xacro:property 0.11993" /> <xacro:property 0.01634" /> <xacro:property 0.11099" /> <xacro:property 0.0" /> name="shoulder_cog" value="0.0 0.00193 name="upper_arm_cog" value="0.0 -0.024201 name="forearm_cog" value="0.0 0.0265 name="wrist_1_cog" value="0.0 0.110949 name="wrist_2_cog" value="0.0 0.0018 name="wrist_3_cog" value="0.0 0.001159 <!-- Kinematic model --> <!-- Properties from urcontrol.conf --> <!-DH for UR5: a = [0.00000, -0.42500, -0.39225, 0.00000, 0.00000, 0.0000] d = [0.089159, 0.00000, 0.00000, 0.10915, 0.09465, 0.0823] alpha = [ 1.570796327, 0, 0, 1.570796327, -1.570796327, 0 ] q_home_offset = [0, -1.570796327, 0, -1.570796327, 0, 0] joint_direction = [-1, -1, 1, 1, 1, 1] mass = [3.7000, 8.3930, 2.2750, 1.2190, 1.2190, 0.1879] center_of_mass = [ [0, -0.02561, 0.00193], [0.2125, 0, 0.11336], [0.11993, 0.0, 0.0265], [0, -0.0018, 0.01634], [0, 0.0018,0.01634], [0, 0, -0.001159] ] --> <xacro:property name="d1" value="0.089159" /> <xacro:property name="a2" value="-0.42500" /> <xacro:property name="a3" value="-0.39225" /> <xacro:property name="d4" value="0.10915" /> <xacro:property name="d5" value="0.09465" /> <xacro:property name="d6" value="0.0823" /> <!-- Arbitrary offsets for shoulder/elbow joints --> <xacro:property name="shoulder_offset" value="0.13585" /> <!-- measured from model --> <xacro:property name="elbow_offset" value="-0.1197" /> <!- measured from model --> <!-- link lengths used in model --> <xacro:property name="shoulder_height" value="${d1}" /> <xacro:property name="upper_arm_length" value="${-a2}" /> <xacro:property name="forearm_length" value="${-a3}" /> <xacro:property name="wrist_1_length" value="${d4 elbow_offset - shoulder_offset}" /> <xacro:property name="wrist_2_length" value="${d5}" /> <xacro:property name="wrist_3_length" value="${d6}" /> <!--property name="shoulder_height" value="0.089159" /--> Electronic copy available at: https://ssrn.com/abstract=3939013 <!--property name="shoulder_offset" value="0.13585" /--> <!-- shoulder_offset - elbow_offset + wrist_1_length = 0.10915 --> <!--property name="upper_arm_length" value="0.42500" /--> <!--property name="elbow_offset" value="0.1197" /--> <!-- CAD measured --> <!--property name="forearm_length" value="0.39225" /--> <!--property name="wrist_1_length" value="0.093" /--> <!-- CAD measured --> <!--property name="wrist_2_length" value="0.09465" /--> <!-- In CAD this distance is 0.930, but in the spec it is 0.09465 --> <!--property name="wrist_3_length" value="0.0823" /--> <xacro:property name="shoulder_radius" value="0.060" /> <!-- manually measured --> <xacro:property name="upper_arm_radius" value="0.054" /> <!-- manually measured --> <xacro:property name="elbow_radius" value="0.060" /> <!-- manually measured --> <xacro:property name="forearm_radius" value="0.040" /> <!-- manually measured --> <xacro:property name="wrist_radius" value="0.045" /> <!-- manually measured --> <link name="${prefix}base_link" > <visual> <geometry> <mesh filename="package://ur_description/meshes/ur5/visual/base.dae" /> </geometry> <material name="LightGrey"> <color rgba="0.7 0.7 0.7 1.0"/> </material> </visual> <collision> <geometry> <mesh filename="package://ur_description/meshes/ur5/collision/base.s tl" /> </geometry> </collision> <xacro:cylinder_inertial radius="0.06" length="0.05" mass="${base_mass}"> <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> <joint name="${prefix}shoulder_pan_joint" type="revolute"> <parent link="${prefix}base_link" /> <child link = "${prefix}shoulder_link" /> <origin xyz="0.0 0.0 ${shoulder_height}" rpy="0.0 0.0 0.0" /> Electronic copy available at: https://ssrn.com/abstract=3939013 <axis xyz="0 0 1" /> <xacro:unless value="${joint_limited}"> <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="3.15"/> <xacro:if value="${safety_limits}"> <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> </xacro:if> </xacro:unless> <xacro:if value="${joint_limited}"> <limit lower="${shoulder_pan_lower_limit}" upper="${shoulder_pan_upper_limit}" effort="150.0" velocity="3.15"/> <xacro:if value="${safety_limits}"> <safety_controller soft_lower_limit="${shoulder_pan_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_pan_upper_limit safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> </xacro:if> </xacro:if> <dynamics damping="0.0" friction="0.0"/> </joint> <link name="${prefix}shoulder_link"> <visual> <geometry> <mesh filename="package://ur_description/meshes/ur5/visual/shoulder. dae" /> </geometry> <material name="LightGrey"> <color rgba="0.7 0.7 0.7 1.0"/> </material> </visual> <collision> <geometry> <mesh filename="package://ur_description/meshes/ur5/collision/should er.stl" /> </geometry> </collision> <xacro:cylinder_inertial radius="0.06" length="0.15" mass="${shoulder_mass}"> <origin xyz="0.0 0.0 0.0" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> <joint name="${prefix}shoulder_lift_joint" type="revolute"> <parent link="${prefix}shoulder_link" /> <child link = "${prefix}upper_arm_link" /> Electronic copy available at: https://ssrn.com/abstract=3939013 <origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 ${pi / 2.0} 0.0" /> <axis xyz="0 1 0" /> <xacro:unless value="${joint_limited}"> <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="3.15"/> <xacro:if value="${safety_limits}"> <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> </xacro:if> </xacro:unless> <xacro:if value="${joint_limited}"> <limit lower="${shoulder_lift_lower_limit}" upper="${shoulder_lift_upper_limit}" effort="150.0" velocity="3.15"/> <xacro:if value="${safety_limits}"> <safety_controller soft_lower_limit="${shoulder_lift_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_lift_upper_limit safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> </xacro:if> </xacro:if> <dynamics damping="0.0" friction="0.0"/> </joint> <link name="${prefix}upper_arm_link"> <visual> <geometry> <mesh filename="package://ur_description/meshes/ur5/visual/upperarm. dae" /> </geometry> <material name="LightGrey"> <color rgba="0.7 0.7 0.7 1.0"/> </material> </visual> <collision> <geometry> <mesh filename="package://ur_description/meshes/ur5/collision/uppera rm.stl" /> </geometry> </collision> <xacro:cylinder_inertial radius="0.06" length="0.56" mass="${upper_arm_mass}"> <origin xyz="0.0 0.0 0.28" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> <joint name="${prefix}elbow_joint" type="revolute"> <parent link="${prefix}upper_arm_link" /> Electronic copy available at: https://ssrn.com/abstract=3939013 <child link = "${prefix}forearm_link" /> <origin xyz="0.0 ${elbow_offset} ${upper_arm_length}" rpy="0.0 0.0 0.0" /> <axis xyz="0 1 0" /> <xacro:unless value="${joint_limited}"> <limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="3.15"/> <xacro:if value="${safety_limits}"> <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> </xacro:if> </xacro:unless> <xacro:if value="${joint_limited}"> <limit lower="${elbow_joint_lower_limit}" upper="${elbow_joint_upper_limit}" effort="150.0" velocity="3.15"/> <xacro:if value="${safety_limits}"> <safety_controller soft_lower_limit="${elbow_joint_lower_limit + safety_pos_margin}" soft_upper_limit="${elbow_joint_upper_limit safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> </xacro:if> </xacro:if> <dynamics damping="0.0" friction="0.0"/> </joint> <link name="${prefix}forearm_link"> <visual> <geometry> <mesh filename="package://ur_description/meshes/ur5/visual/forearm.d ae" /> </geometry> <material name="LightGrey"> <color rgba="0.7 0.7 0.7 1.0"/> </material> </visual> <collision> <geometry> <mesh filename="package://ur_description/meshes/ur5/collision/forear m.stl" /> </geometry> </collision> <xacro:cylinder_inertial radius="0.06" length="${-a3}" mass="${forearm_mass}"> <origin xyz="0.0 0.0 ${-a3/2}" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> <joint name="${prefix}wrist_1_joint" type="revolute"> Electronic copy available at: https://ssrn.com/abstract=3939013 <parent link="${prefix}forearm_link" /> <child link = "${prefix}wrist_1_link" /> <origin xyz="0.0 0.0 ${forearm_length}" rpy="0.0 ${pi / 2.0} 0.0" /> <axis xyz="0 1 0" /> <xacro:unless value="${joint_limited}"> <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="28.0" velocity="3.2"/> <xacro:if value="${safety_limits}"> <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> </xacro:if> </xacro:unless> <xacro:if value="${joint_limited}"> <limit lower="${wrist_1_lower_limit}" upper="${wrist_1_upper_limit}" effort="28.0" velocity="3.2"/> <xacro:if value="${safety_limits}"> <safety_controller soft_lower_limit="${wrist_1_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_1_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> </xacro:if> </xacro:if> <dynamics damping="0.0" friction="0.0"/> </joint> <link name="${prefix}wrist_1_link"> <visual> <geometry> <mesh filename="package://ur_description/meshes/ur5/visual/wrist1.da e" /> </geometry> <material name="LightGrey"> <color rgba="0.7 0.7 0.7 1.0"/> </material> </visual> <collision> <geometry> <mesh filename="package://ur_description/meshes/ur5/collision/wrist1 .stl" /> </geometry> </collision> <xacro:cylinder_inertial radius="0.06" length="0.12" mass="${wrist_1_mass}"> <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> <joint name="${prefix}wrist_2_joint" type="revolute"> <parent link="${prefix}wrist_1_link" /> <child link = "${prefix}wrist_2_link" /> Electronic copy available at: https://ssrn.com/abstract=3939013 <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 0.0 0.0" /> <axis xyz="0 0 1" /> <xacro:unless value="${joint_limited}"> <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="28.0" velocity="3.2"/> <xacro:if value="${safety_limits}"> <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> </xacro:if> </xacro:unless> <xacro:if value="${joint_limited}"> <limit lower="${wrist_2_lower_limit}" upper="${wrist_2_upper_limit}" effort="28.0" velocity="3.2"/> <xacro:if value="${safety_limits}"> <safety_controller soft_lower_limit="${wrist_2_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_2_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> </xacro:if> </xacro:if> <dynamics damping="0.0" friction="0.0"/> </joint> <link name="${prefix}wrist_2_link"> <visual> <geometry> <mesh filename="package://ur_description/meshes/ur5/visual/wrist2.da e" /> </geometry> <material name="LightGrey"> <color rgba="0.7 0.7 0.7 1.0"/> </material> </visual> <collision> <geometry> <mesh filename="package://ur_description/meshes/ur5/collision/wrist2 .stl" /> </geometry> </collision> <xacro:cylinder_inertial radius="0.06" length="0.12" mass="${wrist_2_mass}"> <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0 0 0" /> </xacro:cylinder_inertial> </link> <joint name="${prefix}wrist_3_joint" type="revolute"> <parent link="${prefix}wrist_2_link" /> <child link = "${prefix}wrist_3_link" /> <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" /> Electronic copy available at: https://ssrn.com/abstract=3939013 <axis xyz="0 1 0" /> <xacro:unless value="${joint_limited}"> <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="28.0" velocity="3.2"/> <xacro:if value="${safety_limits}"> <safety_controller soft_lower_limit="${-2.0 * pi + safety_pos_margin}" soft_upper_limit="${2.0 * pi safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> </xacro:if> </xacro:unless> <xacro:if value="${joint_limited}"> <limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}" effort="28.0" velocity="3.2"/> <xacro:if value="${safety_limits}"> <safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/> </xacro:if> </xacro:if> <dynamics damping="0.0" friction="0.0"/> </joint> <link name="${prefix}wrist_3_link"> <visual> <geometry> <mesh filename="package://ur_description/meshes/ur5/visual/wrist3.da e" /> </geometry> <material name="LightGrey"> <color rgba="0.7 0.7 0.7 1.0"/> </material> </visual> <collision> <geometry> <mesh filename="package://ur_description/meshes/ur5/collision/wrist3 .stl" /> </geometry> </collision> <xacro:cylinder_inertial radius="0.0375" length="0.0345" mass="${wrist_3_mass}"> <origin xyz="0.0 ${wrist_3_length - 0.0345/2} 0.0" rpy="${pi/2} 0 0" /> </xacro:cylinder_inertial> </link> <joint name="${prefix}ee_fixed_joint" type="fixed"> <parent link="${prefix}wrist_3_link" /> <child link = "${prefix}ee_link" /> <origin xyz="0.0 ${wrist_3_length} 0.0" rpy="0.0 0.0 ${pi/2.0}" /> </joint> Electronic copy available at: https://ssrn.com/abstract=3939013 <link name="${prefix}ee_link"> <collision> <geometry> <box size="0.01 0.01 0.01"/> </geometry> <origin rpy="0 0 0" xyz="-0.01 0 0"/> </collision> </link> <xacro:ur_arm_transmission prefix="${prefix}" hw_interface="${transmission_hw_interface}" /> <xacro:ur_arm_gazebo prefix="${prefix}" /> <!-- ROS base_link to UR 'Base' Coordinates transform --> <link name="${prefix}base"/> <joint name="${prefix}base_link-base_fixed_joint" type="fixed"> <!-- NOTE: this rotation is only needed as long as base_link itself is not corrected wrt the real robot (ie: rotated over 180 degrees) --> <origin xyz="0 0 0" rpy="0 0 ${-pi}"/> <parent link="${prefix}base_link"/> <child link="${prefix}base"/> </joint> <!-- Frame coincident with all-zeros TCP on UR controller --> <link name="${prefix}tool0"/> <joint name="${prefix}wrist_3_link-tool0_fixed_joint" type="fixed"> <origin xyz="0 ${wrist_3_length} 0" rpy="${pi/-2.0} 0 0"/> <parent link="${prefix}wrist_3_link"/> <child link="${prefix}tool0"/> </joint> </xacro:macro> </robot> ***************************************************************************************** Electronic copy available at: https://ssrn.com/abstract=3939013