Uploaded by Shanmukhasundaram V R

SSRN-id3939013

advertisement
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
Download