project report robot dynamics
TRANSCRIPT
-
8/21/2019 Project Report Robot Dynamics
1/60
Dynamics Simulator for the 6 DOF Stanford Arm
Priyanshu Agarwal
May 13, 2013
Abstract
In this work, we implement the kinematics, dynamics, and control of a 6 Degree Of Free-
dom (DOF) Stanford arm. More specifically, we develop algorithms for path planning and PD
control of the simulated manipulator (plant) (running at 1000Hz) with the end-effector payloadmass (0-0.4 kg) being unknown to the controller (running at 100 Hz). Simulations and vari-
ous checks deployed to validate the accuracy of the implementation show that the simulation
is fully functional. In addition, we carry out a payload mass study by varying its mass and
interpreting the implications on end-effector position tracking. Results show that the system
can manipulate the unknown payload with high accuracy and precision.
Keywords: Path Planning, forward kinematics, inverse kinematics, forward dynamics, inverse
dynamics, Stanford arm
1
-
8/21/2019 Project Report Robot Dynamics
2/60
Contents
1 Program Strucutre 3
1.1 Overall Program Structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.2 Program Flow Chart. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2 Module Testing and Debugging 7
2.1 Inverse-Forward Position Kinematics Testing . . . . . . . . . . . . . . . . . . . . 7
2.2 Inverse-Forward Dynamics Testing . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.3 Miscellaneous Checks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
3 System Level Testing and Debugging 14
3.1 Energy Check . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.2 Miscellaneous Checks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
4 Simulation Results 17
4.1 Problem 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174.1.1 Payload Mass Variation Study . . . . . . . . . . . . . . . . . . . . . . . . 17
4.2 Problem 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
4.2.1 Payload Mass Variation Study . . . . . . . . . . . . . . . . . . . . . . . . 26
4.3 Problem 3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
5 Summary 28
Appendices 29
A MATLAB script for the main program integrating all modules. 29
B MATLAB script for initializing system parameters. 37
C MATLAB function for robot path planning. 39
D MATLAB function for forward position kinematics. 40
E MATLAB script for inverse position kinematics. 41
F MATLAB function for inverse dynamics. 43
G MATLAB function for forward dynamics. 46
H MATLAB function to simulate manipulator dynamics for ode5. 47
I MATLAB function for visualizing stanford arm configuration. 47
J MATLAB function for calculating mass matrix. 49
1
-
8/21/2019 Project Report Robot Dynamics
3/60
K MATLAB function for calculating energy. 52
L MATLAB function for expressingNth coordinate frame in terms of(N 1)th coordi-nate frame using homogeneous transformation. 52
M MATLAB function for solving quadratic equation. 53
N MATLAB function for evaluating spatial cross product. 53
O MATLAB function for evaluating spatial transpose. 53
P MATLAB function for evaluating vectorsi. 53
Q MATLAB function for evaluating matrixU. 54
R MATLAB function for evaluating matrixV. 54
S MATLAB function for expressing a spatial vector in cross form. 54
T MATLAB function for testing inverse and forward position kinematics. 54
U MATLAB function for testing inverse and forward dynamics. 56
2
-
8/21/2019 Project Report Robot Dynamics
4/60
1 Program Strucutre
We developed a highly modular framework to simulate the dynamics of the 6 Degree Of Freedom
(DOF) Stanford Arm for easy debugging and code reusability.
1.1 Overall Program Structure
Figure 1: Overall structure of the program along with the major modules implemented mentioned
in the corresponding block.
Figure1presents the overall structure of the program with associated module mentioned in
each block. The structure can be divided into three major sub-structures: (i) initialization, (ii)
task planning, and (iii) system simulation implementing the controller and the plant. The various
modules defined along with their function are as follows:
1. Main: This is the main module integrating all other modules to simulate the overall system.
(AppendixA)
2. System Parameter Initialization: This module is used to initialize the various plant param-
eters such as link lengths, masses, inertias, DH parameters and properties of the payload
mass. (AppendixB)
3
-
8/21/2019 Project Report Robot Dynamics
5/60
3. Path Planning: This module is used to plan the trajectories for the various joints of the
manipulator given the initial and final configuration of the end-effector using polynomial
interpolation. (AppendixC)
4. Forward Position Kinematics: This module is used to implement the forward position kine-
matics of the manipulator. (AppendixD)
5. Inverse Position Kinematics: This module is used to implement the inverse position kine-
matics of the manipulator. (AppendixE)
6. Inverse Dynamics: This module is used to implement the inverse dynamics of the manipula-
tor. (AppendixF)
7. Forward Dynamics: This moduel is used to implement the forward dynamics of the manip-
ulator. (AppendixG)
8. Manipulator Dynamics: This module is implemented to call the forward dynamics module
for ode5 and display the status of the simulation time while it is running. (AppendixH)
9. Plot Stanford Arm: This module is implemented to plot a 3D visual of the given configura-
tion of the manipulator.(AppendixI)
10. Calculate Mass Matrix: This module is implemented to calculate the mass matrix of the
system. (AppendixJ)
11. Calculate Energy: This module is implemented to calculate the kinetic and potential energy
of the manipulator along with the energy input by the actuators to carry out an energy balance
check. (AppendixK)
12. Homogeneous Transformation: This module is implemented to transform the(N)th coordi-nate frame to(N 1)th coordinate frame using homogeneous transform taking care of boththe coordinate axes orientation and origin location. (AppendixL)
13. Solve Quadratic: This module is developed to solve the quadratic equations which are re-
quired to be solved for the inverse position kinematics. (Appendix M)
14. Spatial Cross: This module is developed to implement the spatial cross product.(Appendix N)
15. Spatial Transpose: This module is implemented to carry out the spatial transpose of the input
vector or matrix. (AppendixO)
16. S Vector: This module is implemented to evaluate the vectorssis needed for inverse dynam-ics. (AppendixP)
17. U matrix: This module is implemented to evaluate the U matrix for coordinate transforma-
tion. (AppendixQ)
4
-
8/21/2019 Project Report Robot Dynamics
6/60
18. V matrix: This module is implemented to evaluate the V matrix for coordinate transforma-
tion. (AppendixR)
19. Cross Form: This module is developed to express a vector in its cross form. (AppendixS)
20. Forward-Inverse Position Kinematics: This module is developed to carry out a through test-
ing of the forward and inverse position kinematics and to check whether the results match
for random inputs. (AppendixT)
21. Forward Inverse Dynamics: This module is implemented to carry out a through test of
the forward and inverse dynamics for a given desired acceleration trajectory and to check
whether the results from the two algorithms match. (AppendixU)
The MATLAB script or function implementing each module is mentioned at the end of each
module description in the aforementioned enumeration.
1.2 Program Flow ChartFigure 2 presents the flow chart of the overall program and the controller and the plant sub-
modules. First, the system parameters such as link lengths, inertias, DH parameters etc. are
initialized. Next, the path planning is carried out for the given initial and final configuration of the
end-effector using the inverse kinematics module. Then a loop for simulation time starts in which
the controller torque consisting of feedforward term (evaluated using inverse dynamics with un-
known payload mass) and a feedback term (evaluated using mass matrix scaled PD terms using the
error on joint positions and velocities) is evaluated. Next, the evaluated torque is used to simulated
the forward dynamics of the system for a controller time step with an increment of the simulator
time step. The joint positions at the end of the current controller time step are then evaluated.
Next, the kinetic, potential and actuator energies are evaluated. The time is then increased by thecontroller time step and checked against the final time. If the time variable is less than the final
simulation time the loop continues, otherwise, the loop ends. Finally, the results are plotted and
the simulation stops.
5
-
8/21/2019 Project Report Robot Dynamics
7/60
(i)
(ii) (iii)
Figure 2: Flow chart of the program along with the flow chart of the sub-modules implemented.
(i) Overall flow chart, (ii) Controller flow chart, (iii) Plant flow chart.
6
-
8/21/2019 Project Report Robot Dynamics
8/60
2 Module Testing and Debugging
In this section, we discuss the various techniques used to test and debug the various modules
individually.
2.1 Inverse-Forward Position Kinematics Testing
Inverse-forward position kinematics tests are carried out to verify the validity and robustness of the
implementation. In this test, a random joint position vector is generated and is used to evaluate the
end-effector location and orientation using forward position kinematics module. The end-effector
location and orientation is then provided to the inverse position kinematics module to generate all
possible 8 solutions. The solution that most closely matches the input joint position vector is then
chosen as the inverse position kinematics result.
Figure3 presents the forward and inverse position kinematics results for a set of 100 random
joint position vectors. As can be seen, the joint positions for all the joints from the forward kine-
matics matches very closely to that obtained from the inverse kinematics. Figure4presents the
error in the results from forward and inverse position kinematics. Again, the errors are very small
of the order of 1013 1016 which exist due to machine precision. This the simulation showsboth the accuracy and robustness of the algorithm.
2.2 Inverse-Forward Dynamics Testing
Inverse-forward dynamics tests are carried out to test the validity and robustness of the inverse and
forward dynamics algorithms. In this test, first, given a desired acceleration trajectory for the var-
ious joints the torques required to achieve the accelerations are evaluated using inverse dynamics
module. Next, using the evaluated torques the joint accelerations are evaluated using the forward
dynamics module. No payload mass is considered to verify the accuracy of the implementation.Since, the required torques are applied the desired and the achieved accelerations should match in
absence of any payload.
Figure5presents the desired and the achieve joint accelerations at the various joints for the
planned trajectory. Results show that both the desired and the achieved accelerations are very
close to each other. Figure6 shows the acceleration error plots for the various joints. As can be
observed, all the errors are of very small order and the acceleration is very noisy.
7
-
8/21/2019 Project Report Robot Dynamics
9/60
0 10 20 30 40 50 60 70 80 90 1001.5
1
0.5
0
0.5
1
1.5
1 vs Time
Time
1
1
1d
(i)
0 10 20 30 40 50 60 70 80 90 1001.5
1
0.5
0
0.5
1
1.5
2 vs Time
Time
2
2
2d
(ii)
0 10 20 30 40 50 60 70 80 90 1001.5
1
0.5
0
0.5
1
1.5
r3 vs Time
Time
r3
r3
r3d
(iii)
0 10 20 30 40 50 60 70 80 90 1001.5
1
0.5
0
0.5
1
1.5
4 vs Time
Time
4
4
4d
(iv)
0 10 20 30 40 50 60 70 80 90 1001.5
1
0.5
0
0.5
1
1.5
5 vs Time
Time
5
5
5d
(v)
0 10 20 30 40 50 60 70 80 90 1001.5
1
0.5
0
0.5
1
1.5
6 vs Time
Time
6
6
6d
(vi)
Figure 3: Forward-inverse position kinematics for 100 random inputs. Generated using MATLAB
script in AppendixT.
.8
-
8/21/2019 Project Report Robot Dynamics
10/60
0 10 20 30 40 50 60 70 80 90 1000.5
0
0.5
1
1.5
2
2.5
3x 10
13 1e vs Time
Time
1
e
(i)
0 10 20 30 40 50 60 70 80 90 1000.5
0
0.5
1
1.5
2
2.5
3x 10
14 2e vs Time
Time
2e
(ii)
0 10 20 30 40 50 60 70 80 90 1006
4
2
0
2
4
6
8
10x 10
16 r3e vs Time
Time
r3e
(iii)
0 10 20 30 40 50 60 70 80 90 1000.5
0
0.5
1
1.5
2
2.5
3
x 1013 4e vs Time
Time
4e
(iv)
0 10 20 30 40 50 60 70 80 90 1000.5
0
0.5
1
1.5
2
2.5
3x 10
14 5e vs Time
Time
5e
(v)
0 10 20 30 40 50 60 70 80 90 1002
1
0
1
2
3
4
5
6x 10
14 6e vs Time
Time
6e
(vi)
Figure 4: Forward-inverse position kinematics error for 100 random inputs. Generated using MAT-
LAB script in AppendixT. 9
-
8/21/2019 Project Report Robot Dynamics
11/60
0 1 2 3 4 5 6 7 8 9 100.2
0.15
0.1
0.05
0
0.05
0.1
0.15
0.2
1 vs Time
Time
1
1
1d
(i)
0 1 2 3 4 5 6 7 8 9 100.03
0.02
0.01
0
0.01
0.02
0.03
2 vs Time
Time
2
2
2d
(ii)
0 1 2 3 4 5 6 7 8 9 102.5
2
1.5
1
0.5
0
0.5
1
1.5
2
2.5x 10
3 r3 vs Time
Time
r3
r3
r3d
(iii)
0 1 2 3 4 5 6 7 8 9 104
3
2
1
0
1
2
3x 10
13 4 vs Time
Time
4
4
4d
(iv)
0 1 2 3 4 5 6 7 8 9 100.03
0.02
0.01
0
0.01
0.02
0.03
5 vs Time
Time
5
5
5d
(v)
0 1 2 3 4 5 6 7 8 9 100.2
0.15
0.1
0.05
0
0.05
0.1
0.15
0.2
6 vs Time
Time
6
6
6d
(vi)
Figure 5: Forward-inverse dynamics acceleration results for the planned trajectory with a payload
mass of 0 kg. Generated using MATLAB script in AppendixU.
.10
-
8/21/2019 Project Report Robot Dynamics
12/60
0 1 2 3 4 5 6 7 8 9 102
1.5
1
0.5
0
0.5
1
1.5
2
2.5x 10
14 1e vs Time
Time
1
e
(i)
0 1 2 3 4 5 6 7 8 9 101.5
1
0.5
0
0.5
1
1.5x 10
14 2e vs Time
Time
2
e
(ii)
0 1 2 3 4 5 6 7 8 9 103
2
1
0
1
2x 10
14 r3e vs Time
Time
r3e
(iii)
0 1 2 3 4 5 6 7 8 9 104
3
2
1
0
1
2
3x 10
13 4e vs Time
Time
4e
(iv)
0 1 2 3 4 5 6 7 8 9 106
4
2
0
2
4
6
8x 10
13 5e vs Time
Time
5e
(v)
0 1 2 3 4 5 6 7 8 9 1010
8
6
4
2
0
2
4
6
8x 10
14 6e vs Time
Time
6e
(vi)
Figure 6: Forward-inverse dynamics acceleration error results for the planned trajectory with a
payload mass of 0 kg. Generated using MATLAB script in AppendixU.
.11
-
8/21/2019 Project Report Robot Dynamics
13/60
2.3 Miscellaneous Checks
We also used the following checks to ensure the validity of the individual modules:
1. We used the various equations used to solve for the joint inverse position kinematics (List-
ing1) as checks to verify whether the evaluated joint positions satisfy the equations solved to
obtain them. For example, Eq. 1 is used as a check in the module inverse position kinematics.(AppendixE).
cos 5cos 6= p11cos 4 p21sin 4 (1)
Listing 1: Code snippet to check for the various equations.
% Equation Checks
i f( abs( c o s (theta_5A)* c o s (theta_6A)-P(1,1)* c o s (theta_4A)-P(2,1)*...
s i n (theta_4A))>10(-10))
d i s p (Inverse Position Kinematics: Check 1 Failed);
5 pa us e;
en d
i f( abs( c o s (theta_5A)* s i n (theta_6A)+P(1,2)* c o s (theta_4A)+P(2,2)*...
s i n (theta_4A))>10(-10))
10 d i s p (Inverse Position Kinematics: Check 2 Failed);
pa us e;
en d
i f( abs( s i n (theta_5A)* c o s (theta_6A)-P(3,1))>10(-10))
15 d i s p (Inverse Position Kinematics: Check 3 Failed);
pa us e;
en d
i f( abs( s i n (theta_5A)* s i n (theta_6A)+P(3,2))>10(-10))
20 d i s p (Inverse Position Kinematics: Check 4 Failed);
pa us e;
en d
2. We also used the check of inertia tensor being symmetric with respect to the spatial transpose
(Listing2) and mass matrix with respect to the matrix transpose (Listing2) i.e.
IS = I (2)
MT
=M
(3)
Listing 2: Code snippet to check for the symmetry of the inertia tensor with respect to the spatial
transpose.
% I matrix symmetry check wrt spatial transpose
12
-
8/21/2019 Project Report Robot Dynamics
14/60
i f(max(max( abs(I0c_i_hat(:,:,i)-...
spatial_transpose(I0c_i_hat(:,:,i)))))>10E-10)
warning([Forward Dynamics -> I matrix not symmetric,...
5 wrt Spatial Transpose!]);
pa us e;
en d
Listing 3: Code snippet to check for the symmetry of the mass matrix with respect to the matrix
transpose.
% M_matrix symmetry check
i f(max(max( abs(M_matrix-M_matrix)))>10E-10)
warning(Forward Dynamics -> M matrix not symmetric!);
pa us e;
5 en d
3. Furthermore, we used the check on the eigenvalues of the rotation matrix all of which shouldhave a magnitude of 1.
Listing 4: Code snippet to check for the eigenvalues of the rotation matrix.
% If the eigen values of the rotation matrix are not 1
i f( abs(max( abs( e i g (A0_i(1:3,1:3,i))))-1)>10E-9)
warning(Inverse Dynamics -> Rotation matrix invalid!);
en d
13
-
8/21/2019 Project Report Robot Dynamics
15/60
3 System Level Testing and Debugging
In this section, we present the various techniques used to test and debug the integrated system.
3.1 Energy Check
Energy balance check is one of the most crucial checks to verify the validity of the implemented
algorithms and their integration. As per this check, the energy in the system should be conserved
at any point in time. So, the total energy of the system given by the sum of its kinetic and potential
energy (Eq.5) at any time should be equal to the sum of the initial total energy of the system and
the energy input by the actuators (Eq.4). Furthermore, to more closely examine any discrepancy
(if any) in the energy, we also considered the differential form of the equation (Eq.6).
U(t) =U(0) +
t0
()T()d (4)
U(t) =
6j=1
1
2 vsj Ij vj+
6j=1
mjgcjz (5)
Also,
dU(t) =()T()d (6)
0 2 4 6 8 100.5
0
0.5
1
1.5
2
2.5
3
3.5
4x 10
4Instantaneous Energy Difference vs Time
Time
Energy
(Joules)
Total Energy Diff
Instantaneous Actuator Energy Input
(i)
0 2 4 6 8 100
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2
Energy Check vs Time
Time
Energy
(Joules)
Total Energy
Actuator Input Energy
(ii)
Figure 7: Energy differential, total system and actuator energy, and energy error plots for 10 second
simulation of the planned path. (contd.)
14
-
8/21/2019 Project Report Robot Dynamics
16/60
0 2 4 6 8 102
1.5
1
0.5
0
0.5
1x 10
4 Energy Error vs Time
Time
EnergyError(Joule
s)
(iii)
Figure 7: Energy differential, total system and actuator energy, and energy error plots for 10 second
simulation of the planned path.
Figure7 presents the results of the energy check carried out for the 10 second simulation for
the planned path. Figure7iplots both the sides of Eqn. 6and Figure7iiplots both the sides of
Eqn.4.As can be seen the two sides of both the equations match. Figure7iiishows the plot of the
error in energy i.e. discrepancy between the left and right hand side of Eq. 4. There is small error
in energy due to errors introduced by numerical approximation of integration. Hence, the energy
is conserved during the simulation.
3.2 Miscellaneous Checks
1. We used a check on the derivative state vector of the system (Listing5) while integrating
using ode5 to check for the cause of diverging states (if any) to generate warning and quickly
debug the cause of such an error (AppendixH).
Listing 5: Code snippet to check for large values of state derivative during ode5 integration.
% Check
i f(max( abs(X_dot))>1)warning(Large X_dot);
en d
2. We also used the MATLAB in-build error and warning handling (Listing6) to check for the
source of the error and warning for debugging and initiate automatic enabling of break point
15
-
8/21/2019 Project Report Robot Dynamics
17/60
at any such occurrence (AppendixA).
Listing 6: Code snippet to enable MATLAB in-build error and warning handling.
%% Error/Warning Handling
d bs to p i f warning
d bs to p i f e r ro r
16
-
8/21/2019 Project Report Robot Dynamics
18/60
4 Simulation Results
In this section, we present the simulation results and also present answers to the questions posed
in the three problems.
4.1 Problem 1
Figure8presents the configuration of the Stanford arm during the given path tracking task with
a payload mass of 0.4 kg unknown to the controller. Figures 9 and 10 present the joint angle
and velocity tracking results, respectively. Figures11 and 12 present the joint angle and velocity
tracking error results, respectively. As can be seen, the actual trajectories very closely match the
desired joint position and velocity trajectories. In general, the velocity error magnitudes are more
than the errors in position.
Figure13presents the feedforward and feedback torque required and the end-effector position
tracking results. It can be observed, that a major portion of the torque requirement is met through
the feedforward control which shows that the controller model closely explains the simulator with
the payload being the only uncertainty. The position plot of the end-effector also shows that its
initial and final position match with the corresponding desired positions. Finally, the error plot of
the end-effector position shows that the error in position tracking of the end-effector is of the order
of 108m.
4.1.1 Payload Mass Variation Study
We also carry out a payload mass variation study in which the effect of the unknown payload
mass on the end-effector tracking in evaluated. It is observed that as the unknown payload mass
is gradually increased, the peak error also increases due to controller model being more inaccurate
representation of the actual simulator model (Figure14). Note the change in the order of the error
magnitude with the variation in the payload mass.
17
-
8/21/2019 Project Report Robot Dynamics
19/60
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0Y0
t = 0 sec
Z
0
(i)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0Y0
t = 1 sec
Z
0
(ii)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0Y0
t = 2 sec
Z0
(iii)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0Y0
t = 3 sec
Z0
(iv)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0Y0
t = 4 sec
Z0
(v)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0Y0
t = 5 sec
Z0
(vi)
Figure 8: Stanford arm configuration at various time instants during the path tracking task. The
time for each configuration is mentioned in the figure. (contd.)
18
-
8/21/2019 Project Report Robot Dynamics
20/60
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0Y0
t = 6 sec
Z0
(vii)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0Y0
t = 7 sec
Z0
(viii)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0Y0
t = 8 sec
Z0
(ix)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0Y0
t = 9 sec
Z0
(x)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0Y0
t = 10 sec
Z0
(xi)
Figure 8: Stanford arm configuration at various time instants during the path tracking task. The
time for each configuration is mentioned in the figure.
19
-
8/21/2019 Project Report Robot Dynamics
21/60
0 1 2 3 4 5 6 7 8 9 100.5
0
0.5
1
1.5
2
2.5
3
1 vs Time
Time
1
1
1d
(i)
0 1 2 3 4 5 6 7 8 9 102.05
2
1.95
1.9
1.85
1.8
1.75
1.7
1.65
1.6
1.55
2 vs Time
Time
2
2
2d
(ii)
0 1 2 3 4 5 6 7 8 9 100.445
0.45
0.455
0.46
0.465
0.47
0.475
0.48
0.485
0.49
r3 vs Time
Time
r3
r3
r3d
(iii)
0 1 2 3 4 5 6 7 8 9 103
2
1
0
1
2
3
4x 104 4 vs Time
Time
4
4
4d
(iv)
0 1 2 3 4 5 6 7 8 9 101.1
1.15
1.2
1.25
1.3
1.35
1.4
1.45
1.5
1.55
1.6
5 vs Time
Time
5
5
5d
(v)
0 1 2 3 4 5 6 7 8 9 100.5
0
0.5
1
1.5
2
2.5
3
6 vs Time
Time
6
6
6d
(vi)
Figure 9: Position tracking results with payload mass of 0.4 kg.
20
-
8/21/2019 Project Report Robot Dynamics
22/60
0 1 2 3 4 5 6 7 8 9 100.7
0.6
0.5
0.4
0.3
0.2
0.1
0
0.1
1 vs Time
Time
1
1
1d
(i)
0 1 2 3 4 5 6 7 8 9 100.09
0.08
0.07
0.06
0.05
0.04
0.03
0.02
0.01
0
0.01
2 vs Time
Time
2
2
2d
(ii)
0 1 2 3 4 5 6 7 8 9 109
8
7
6
5
4
3
2
1
0
1x 103 r3 vs Time
Time
r3
r3
r3d
(iii)
0 1 2 3 4 5 6 7 8 9 102
1
0
1
2
3
4x 10
4
4 vs Time
Time
4
4
4d
(iv)
0 1 2 3 4 5 6 7 8 9 100.09
0.08
0.07
0.06
0.05
0.04
0.03
0.02
0.01
0
0.01
5 vs Time
Time
5
5
5d
(v)
0 1 2 3 4 5 6 7 8 9 100.1
0
0.1
0.2
0.3
0.4
0.5
0.6
6 vs Time
Time
6
6
6d
(vi)
Figure 10: Velocity tracking results for payload mass of 0.4 kg.
21
-
8/21/2019 Project Report Robot Dynamics
23/60
0 1 2 3 4 5 6 7 8 9 102
1.5
1
0.5
0
0.5
1
1.5x 10
5 1e vs Time
Time
1
e
(i)
0 1 2 3 4 5 6 7 8 9 1010
8
6
4
2
0
2x 10
6 2e vs Time
Time
2e
(ii)
0 1 2 3 4 5 6 7 8 9 1016
14
12
10
8
6
4
2
0
2x 10
6 r3e vs Time
Time
r3e
(iii)
0 1 2 3 4 5 6 7 8 9 103
2
1
0
1
2
3
4x 10
4 4e vs Time
Time
4e
(iv)
0 1 2 3 4 5 6 7 8 9 101
0
1
2
3
4
5
6
7x 10
4 5e vs Time
Time
5e
(v)
0 1 2 3 4 5 6 7 8 9 1010
8
6
4
2
0
2x 10
5 6e vs Time
Time
6e
(vi)
Figure 11: Position tracking error results with payload mass of 0.4 kg.
22
-
8/21/2019 Project Report Robot Dynamics
24/60
0 1 2 3 4 5 6 7 8 9 100.7
0.6
0.5
0.4
0.3
0.2
0.1
0
0.1
1e vs Time
Time
1
e
(i)
0 1 2 3 4 5 6 7 8 9 100.09
0.08
0.07
0.06
0.05
0.04
0.03
0.02
0.01
0
0.01
2e vs Time
Time
2e
(ii)
0 1 2 3 4 5 6 7 8 9 109
8
7
6
5
4
3
2
1
0
1x 103 r3
e vs Time
Time
r3e
(iii)
0 1 2 3 4 5 6 7 8 9 104
3
2
1
0
1
2x 10
4
4e vs Time
Time
4e
(iv)
0 1 2 3 4 5 6 7 8 9 100.09
0.08
0.07
0.06
0.05
0.04
0.03
0.02
0.01
0
0.01
5e vs Time
Time
5e
(v)
0 1 2 3 4 5 6 7 8 9 100.1
0
0.1
0.2
0.3
0.4
0.5
0.6
6e vs Time
Time
6e
(vi)
Figure 12: Velocity tracking error results with payload mass of 0.4 kg.
23
-
8/21/2019 Project Report Robot Dynamics
25/60
0 2 4 6 8 105
4
3
2
1
0
1
2
3
4
5
FeedForward Torque vs Time
Time
FeedForwardTorque(Nm)
1
2
f3
4
5
6
(i)
0 2 4 6 8 103
2
1
0
1
2
3
4
5x 10
3Feedback Torque vs Time
Time
Fee
dbackTorque(Nm)
1
2
f3
4
5
6
(ii)
0 1 2 3 4 5 6 7 8 9 100.5
0.4
0.3
0.2
0.1
0
0.1
0.2
0.3
0.4
0.5End Effector Position vs Time
Time
p0 e
(m)
Radius of minimum size container is 2.8465e07
X
Y
Z
(iii)
0 1 2 3 4 5 6 7 8 9 104
3
2
1
0
1
2
3x 10
5 EE Position Error vs Time
Time
p0 e
error(m)
X
Y
Z
(iv)
Figure 13: Actuator torque (feedforward and feedback), end-effector position and end-effector
position error results with payload mass of 0.4 kg.
24
-
8/21/2019 Project Report Robot Dynamics
26/60
0 1 2 3 4 5 6 7 8 9 108
6
4
2
0
2
4
6x 10
7 EE Position Error vs Time
Time
p0 e
error(m)
X
Y
Z
(i)payload mass = 0 gm
0 2 4 6 8 102
1.5
1
0.5
0
0.5
1
1.5x 10
5 EE Position Error vs Time
Time
p0 e
error(m)
X
Y
Z
(ii)payload mass = 100 gm
0 2 4 6 8 103
2.5
2
1.5
1
0.5
0
0.5
1
1.5
2x 10
5 EE Position Error vs Time
Time
p0 e
error(m)
X
Y
Z
(iii)payload mass = 200 gm
0 1 2 3 4 5 6 7 8 9 104
3
2
1
0
1
2
3x 10
5 EE Position Error vs Time
Time
p0 e
error(m)
X
Y
Z
(iv)payload mass = 300 gm
0 1 2 3 4 5 6 7 8 9 104
3
2
1
0
1
2
3x 10
5 EE Position Error vs Time
Time
p0 e
error(m)
X
Y
Z
(v)payload mass = 400 gm
Figure 14: Variation of end-effector tracking error with change in the payload mass for original
end-effector end position and point mass.
25
-
8/21/2019 Project Report Robot Dynamics
27/60
4.2 Problem 2
Once you have performed the inverse position kinematics at the initial and the final posi-
tions, obtain a new final position that results from reducing the angular displacements at
each joint to10%of its original values. Next, assume that the payload of unknown mass M isa spherical object of diameter 0.05 m centered at Point E. Also assume that there is a cylin-
derical container whose axis is parallel to axis 1 of the robot and it passes through point E.
This container has a diameter of 0.07m.
1. Tune the contoller so that the payload can be dropped into the container.
In order to tune the controller gains for each joint appropriately, we multiply the chosen PD
gains with the mass matrix of the system and evalue the feedback torque (fd) as follows:
fd = MKp
Q
dQ
+ Kv
Q
d Q
(7)
Kp= 3556 I66 (8)
Kv= 101 I66 (9)
0 1 2 3 4 5 6 7 8 9 100.2
0.1
0
0.1
0.2
0.3
0.4
0.5End Effector Position vs Time
Time
p0 e
(m)
Radius of minimum size container is 3.4742e08
X
Y
Z
Figure 15: End-effector position for the scaled joint displacements with a payload mass of 0.4 kg.
2. What is the smallest diameter container you can drop the spherical object into?
The smallest diameter container in which the spherical object can be dropped is 3.4742108m.As can be seen, the payload can be very precisely placed in the container.
4.2.1 Payload Mass Variation Study
We also carried out a payload mass study with the scaled joint displacements and spherical payload
(Figure16). Here also the results show the same trend as discussed in Section4.1.1.
26
-
8/21/2019 Project Report Robot Dynamics
28/60
0 1 2 3 4 5 6 7 8 9 102
1
0
1
2
3
4
5
6
7x 10
8 EE Position Error vs Time
Time
p0 e
error(m)
X
Y
Z
(i)payload mass = 0 gm
0 1 2 3 4 5 6 7 8 9 108
6
4
2
0
2
4
6
8x 10
7 EE Position Error vs Time
Time
p0 e
error(m)
X
Y
Z
(ii)payload mass = 100 gm
0 2 4 6 8 101.5
1
0.5
0
0.5
1
1.5x 10
6 EE Position Error vs Time
Time
p0 e
error(m)
X
Y
Z
(iii)payload mass = 200 gm
0 2 4 6 8 102
1.5
1
0.5
0
0.5
1
1.5
2x 10
6 EE Position Error vs Time
Time
p0 e
error(m)
X
Y
Z
(iv)payload mass = 300 gm
0 2 4 6 8 102
1.5
1
0.5
0
0.5
1
1.5
2x 10
6 EE Position Error vs Time
Time
p0 e
error(m)
X
Y
Z
(v)payload mass = 400 gm
Figure 16: Variation of end-effector tracking error with change in the payload mass for scaled joint
displacement and spherical payload.27
-
8/21/2019 Project Report Robot Dynamics
29/60
4.3 Problem 3
Next, discuss the major enhancements that you will have to incorporate into your simulator
if you had to perform a realistic simulation of the system shown below in the context of a
robotic vehicle collecting a geological sample and placing it in a container that is located on
the vehicle body.
We believe that the following changes would need to be incorporated into the current simulation
before an accurate simulation of the presented system can be carried out:
1. Kinematics Formulation: The closed loop kinematic chain formulation would need to be
adopted for the 3 fingered gripper as the three fingers hold the geological sample and would
be constrained assuming that the sample is steadily held in the gripper. Also, the three
wheels of the mobile robot would form a closed kinematic chain with the terrain. Since, the
system has redundant degrees of freedom, an optimization based scheme would need to be
employed to solve for the inverse kinematics of the system.
2. Path Planning: In addition to the robotic manipulator path planning, the mobile robot path
planning would also need to be performed before a task can be accomplished. This mightitself require optimization to choose for the optimum path among the many available paths
based on obstacles on the terrain.
3. Dynamics Formulation: Since, the manipulator is mounted on a moving base, the moving
base formulation which accounts for the acceleration of the base frame would need to be
implemented for forward and inverse dynamics. In addition, since the ground reaction forces
are unknown, the hybrid dynamics approach would need to be employed to solve for the
unknown accelerations and the ground reaction forces (Eq. 10).
M11 OM21 1
1
=10b1+ M12
2
b2+ M222
(10)
4. Unknown Payload Mass and Inertia: Since, the geological samples are collected in the mo-
bile robot itself, the mass and inertia of the robot would change as the samples are collected.
So, there will be more elements of uncertainty, in addition to the uncertainty due to end-
effector payload mass and inertia.
5. Non-holonomic Constraints: Since, the mobile robot has three wheels with steerable front
wheel, the non-holonomic constraints introduced due to the no slip condition of the wheels
on the even terrain would need to be incorporated while solving for the dynamics of the
system using techniques such as Lagrange multiplier [2,1].
5 Summary
In this work, we developed a dynamics based simulation of the 6 DOF Stanford Arm robot. Sim-
ulation results along with the various checks employed to validate the accuracy of the simulation
28
-
8/21/2019 Project Report Robot Dynamics
30/60
showed that the system is fully functional.
References
[1] B. dAndrea Novel, G. Bastin, and G. Campion. Dynamic feedback linearization of nonholo-nomic wheeled mobile robots. InIEEE International Conference on Robotics and Automation,
pages 25272532, 1992.
[2] Qing Yu, I-Ming Chen, et al. A general approach to the dynamics of nonholonomic mobile ma-
nipulator systems. JOURNAL OF DYNAMIC SYSTEMS MEASUREMENT AND CONTROL,
124(4):512521, 2002.
Appendices
A MATLAB script for the main program integrating all mod-ules.
f u n c t i o n main
% MATLAB Code for simulating the motion planning, inverse and forward
% dynamics computations of 6 DOF Stanford Arm.
% INPUTS : Initial and final position and orientation of the robot
5 % end-effector
% PARAMETERS : Robot Geometric and Inertial Parameters
% OUTPUTS : Joint position and velocity tracking results along with
% actuator torque and energy plots.
% AUTHOR : Priyanshu Agarwal
10
%% INITIAL CLEANING
% ==============================
c l e a r ;
15 c l c ;
%% Error/Warning Handling
d bs to p i f warning
d bs to p i f e r ro r
20
%% Figure print setup
c l e a r
% User-defined Parameters
25 print_flag = 1; % all plots will be printed as pdf if 1
% (only valid if animation_flag = 1)
i f print_flag
addpath(/home/priyanshu/Dropbox/MATLAB/export_fig);
29
-
8/21/2019 Project Report Robot Dynamics
31/60
30
% Directory to print the pdf files
print_dir = [/home/priyanshu/Dropbox/UT_Courses/2013_Spring/,...
Advanced_Dynamics_of_Robotic_Systems/Project/Project_Report/figures];
fontsize = 18;
35 e l s e
fontsize = 14;en d
%% INITIALISATION AND PARAMETER DEFINITION:
40 % =========================================
% System Parameters
system_parameters_plant;
% Controller Variables
45 fc = 100; % controller frequency in Hz
dtc = 1/fc; % time step of controller
% Controller gains
K_p = 3556* e y e (6);50 K_v = 101* e y e (6);
% Simulator Variables
fs = 1000; % forward dynamics simulator frequency in Hz
dts = 1/fs; % time step of forward dynamics simulator
55
% flag to turn on or off the planned path simulation
path_simulation_flag = 0;
% flag to turn on or off the printing of the animation
print_animation_flag = 0;
60 %% Plan Path
p0_E_i = [0.5; 0; -0.1];
Q_i = z e r o s (6,1);
Q_dot_i = z e r o s (6,1);
Q_ddot_i = z e r o s (6,1);
65
p0_E_f = [-0.4; 0.1; 0.1];
Q_f = z e r o s (6,1);
Q_dot_f = z e r o s (6,1);
Q_ddot_f = z e r o s (6,1);
70 ti = 0;
tf = 10;
path_scale = 1; % path scale factor to determine what percentage of the
% path needs to be planned
75 [Q_c,Q_i,Q_f] = plan_path(p0_E_i,p0_E_f,Q_dot_i,Q_dot_f,Q_ddot_i,...
Q_ddot_f,ti,tf, P_DH,path_scale);
% Simulating planned path
i f path_simulation_flag
80 f o r t=ti:0.1:tf
30
-
8/21/2019 Project Report Robot Dynamics
32/60
Q_d = Q_c*[1;t;t2;t3;t4;t5];
plot_stanford_arm(Q_d,P_DH);
pa us e(0.01);
en d
85 en d
tstart = ti;tend = tf;
90 T = [tstart:dts:tend];
%% Actual Simulation
% Changing payload mass
f o r pm_mass=0.4:-0.1:0
95
Mps.mass = pm_mass;
i f print_flag
prefix = [/result_PS num2str(path_scale*10) _PM,...
num2str(Mps.mass*10) _];
100 en dX = z e r o s ( l e n g t h (T),12);
Tau_ff_matrix = z e r o s (6, l e n g t h (T));
Tau_fd_matrix = z e r o s (6, l e n g t h (T));
105 KE = z e r o s ( s i z e (T));
PE = z e r o s ( s i z e (T));
AE = z e r o s ( s i z e (T));
P0E = z e r o s ( l e n g t h (T),3);
P0E_d = z e r o s ( l e n g t h (T),3);
110
i f(print_animation_flag)
plot_stanford_arm(Q_i,P_DH);
t e x t (-0.4,0.4,0.4,[ t = num2str(ti) sec],FontName,...
Times New Roman,FontSize,14);
115 export_fig([print_dir prefix config num2str(round(ti))],...
-pdf,-transparent);
en d
f o r t=tstart:dtc:tend-dtc
120
T_s = t:dts:t+dtc;
i f(t==0)
Q_s = Q_i;
Q_dot_s = Q_dot_i;
125
e l s e Q_s = X_s(end ,1:6);
Q_dot_s = X_s(end ,7:12);
en d
130 % Evaluating desired joint angles, velocities and accelerations
Q_d = Q_c*[1;t;t2;t3;t4;t5];
31
-
8/21/2019 Project Report Robot Dynamics
33/60
Q_dot_d = Q_c*[0;1;2*t;3*t2;4*t3;5*t4];
Q_ddot_d = Q_c*[0;0;2;6*t;12*t2;20*t3];
% err_integral = err_integral+(Q_d-Q_s)*dtc;
135
% Calculating M_matrix as global variable
[,,M_matrix,,,] = calculate_mass_matrix(Q_s,P_DH,Mpc);
% Evaluating controller torque
140 Tau_ff = inverse_dynamics(Q_s,Q_dot_s,Q_ddot_d,P_DH,Mpc);
Tau_fd = M_matrix*(K_v*(Q_dot_d-Q_dot_s)+K_p*(Q_d-Q_s));
Tau = Tau_ff + Tau_fd;
[X_s] = ode5(@(t,X) manipulator_dynamics(t,X,P_DH,Mps,Tau),...
145 T_s,[Q_s;Q_dot_s]);
i f(print_animation_flag && (rem(T_s(en d),1)==0))
plot_stanford_arm(X_s(end ,1:6),P_DH);
t e x t (-0.4,0.4,0.4,[ t = num2str(round(T_s(en d))) sec],...
150 FontName,Times New Roman,FontSize,14);
export_fig([print_dir prefix config,...
num2str(round(T_s(en d)*fc))],...
-pdf,-transparent);
en d
155
X(round(t*fs)+1:round(t*fs)+ l e n g t h (T_s),:) = X_s;
Tau_ff_matrix(:,round(t*fs)+1:round(t*fs)+ l e n g t h (T_s)) =...
repmat(Tau_ff,1,11);
Tau_fd_matrix(:,round(t*fs)+1:round(t*fs)+ l e n g t h (T_s)) =...
160 repmat(Tau_fd,1,11);
en d
Q_d = Q_c*[ones( s i z e (T));T;T.2;T.3;T.4;T.5];
165 Q_dot_d = Q_c*...
[ z e r o s ( s i z e (T));ones( s i z e (T));2*T;3*T.2;4*T.3;5*T.4];
% for i=1:100:size(X,1)
% Q = X(i,1:6);
170 % plot_stanford_arm(Q,P_DH);
% pause(0.001);
% end
f o r i=1:1: s i z e (X,1)
175 Q = X(i,1:6);
Q_dot = X(i,7:12);[ke,pe,ae] = calculate_energy(Q,Q_dot,P_DH,Mps,dts,...
Tau_ff_matrix(:,i)+Tau_fd_matrix(:,i));
KE(i) = ke;
180 PE(i) = pe;
AE(i) = ae;
[r0_6,] = forward_position_kinematics(Q, P_DH);
32
-
8/21/2019 Project Report Robot Dynamics
34/60
P0E(i,:) = r0_6;
[r0_6_d,] = forward_position_kinematics(Q_d(:,i), P_DH);
185 P0E_d(i,:) = r0_6_d;
en d
% [r0_6_d,~] = forward_position_kinematics(Q_d(:,end), P_DH);190
min_rc = norm(r0_6(1:2)-r0_6_d(1:2));
d i s p ([Radius of minimum size container is num2str(min_rc)]);
%% PLOTTING RESULTS
195 str = {$\theta_1$,$\theta_2$,$r_3$,...
$\theta_4$,$\theta_5$,$\theta_6$,...
$\dot{\theta}_1$,$\dot{\theta}_2$,$\dot{r}_3$,...
$\dot{\theta}_4$,$\dot{\theta}_5$,$\dot{\theta}_6$,...
$\theta_{1e}$,$\theta_{2e}$,$r_{3e}$,...
200 $\theta_{4e}$,$\theta_{5e}$,$\theta_{6e}$,...
$\dot{\theta}_{1e}$,$\dot{\theta}_{2e}$,$\dot{r}_{3e}$,...
$\dot{\theta}_{4e}$,$\dot{\theta}_{5e}$,$\dot{\theta}_{6e}$};
strd = {$\theta_{1d}$,$\theta_{2d}$,$r_{3d}$,...
205 $\theta_{4d}$,$\theta_{5d}$,$\theta_{6d}$,...
$\dot{\theta}_{1d}$,$\dot{\theta}_{2d}$,$\dot{r}_{3d}$,...
$\dot{\theta}_{4d}$,$\dot{\theta}_{5d}$,$\dot{\theta}_{6d}$};
% theta
210 f o r i=1:6
f i g u r e (i+1);
c l f
p l o t (T, X(1: l e n g t h (T),i),-b,T, Q_d(i,:),-r,LineWidth,2);
g r i d on
215 t i t l e ([str{i} vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
x l a b e l (Time $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
y l a b e l ([str{i} $\rightarrow$],Interpreter,latex,...
220 FontName,...
Times New Roman,FontSize,fontsize);
l e g e n d ({str{i},strd{i}},...
Interpreter,latex,FontName,Times New Roman,...
FontSize,12,...
225 Color,none,Location,Best);
en d
% theta_dot
f o r i=1:6
230 f i g u r e (i+7);
c l f
p l o t (T, X(1: l e n g t h (T),i+6),-b,T, Q_dot_d(i,:),-r,...
LineWidth,2);
33
-
8/21/2019 Project Report Robot Dynamics
35/60
g r i d on
235 t i t l e ([str{i+6} vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
x l a b e l (Time $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
y l a b e l ([str{i+6} $\rightarrow$],Interpreter,latex,...
240 FontName,Times New Roman,FontSize,fontsize);l e g e n d ({str{i+6},strd{i+6}},...
Interpreter,latex,FontName,Times New Roman,...
FontSize,12,...
Color,none,Location,Best);
245 en d
% theta_error
f o r i=1:6
f i g u r e (i+13);
250 c l f
p l o t (T, X(1: l e n g t h (T),i)-Q_d(i,:),-b,LineWidth,2);
g r i d on
t i t l e ([str{i+12} vs Time],Interpreter,latex,...FontName,Times New Roman,FontSize,fontsize);
255 x l a b e l (Time $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
y l a b e l ([str{i+12} $\rightarrow$],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
en d
260
% theta_dot_error
f o r i=1:6
f i g u r e (i+19);
c l f
265 p l o t (T, X(1: l e n g t h (T),i+6)-Q_dot(i,:),-b,LineWidth,2);
g r i d on
t i t l e ([str{i+18} vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
x l a b e l (Time $\rightarrow$,Interpreter,latex,FontName,...
270 Times New Roman,FontSize,fontsize);
y l a b e l ([str{i+18} $\rightarrow$],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
en d
275 % Energy difference
i = i+20;
f i g u r e (i);
c l fp l o t (T(2:en d), d i f f (KE)+ d i f f (PE),-b,T,AE,-k,LineWidth,2);
280 g r i d on
t i t l e ([Instantaneous Energy Difference vs Time],Interpreter,...
latex,...
FontName,Times New Roman,FontSize,fontsize);
x l a b e l (Time $\rightarrow$,Interpreter,latex,FontName,...
34
-
8/21/2019 Project Report Robot Dynamics
36/60
285 Times New Roman,FontSize,fontsize);
y l a b e l ([Energy (Joules) $\rightarrow$],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
l e g e n d ({Total Energy Diff,Instantaneous Actuator Energy Input},...
Interpreter,latex,FontName,Times New Roman,...
290 FontSize,12,...
Color,none,Location,Best);
% Energy
i = i+1;
295 f i g u r e (i);
c l f
p l o t (T, KE+PE-KE(1)-PE(1),-b,T,cumsum(AE),-k,LineWidth,2);
g r i d on
t i t l e ([Energy Check vs Time],Interpreter,latex,...
300 FontName,Times New Roman,FontSize,fontsize);
x l a b e l (Time $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
y l a b e l ([Energy (Joules) $\rightarrow$],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
305 l e g e n d ({Total Energy,Actuator Input Energy},...
Interpreter,latex,FontName,Times New Roman,...
FontSize,12,...
Color,none,Location,Best);
310 % Energy Error
i = i+1;
f i g u r e (i);
c l f
p l o t (T, KE+PE-KE(1)-PE(1)-cumsum(AE),-b,LineWidth,2);
315 g r i d on
t i t l e ([Energy Error vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
x l a b e l (Time $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
320 y l a b e l ([Energy Error (Joules) $\rightarrow$],Interpreter,...
latex,...
FontName,Times New Roman,FontSize,fontsize);
% Joint Feedforward Torques/Forces
325 i = i+1;
f i g u r e (i);
c l f
p l o t (T, Tau_ff_matrix(1,:),b,T, Tau_ff_matrix(2,:),r,...
T, Tau_ff_matrix(3,:),g,T, Tau_ff_matrix(4,:),k,...330 T, Tau_ff_matrix(5,:),c,T, Tau_ff_matrix(6,:),m,LineWidth,2);
g r i d on
t i t l e ([FeedForward Torque vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
x l a b e l (Time $\rightarrow$,Interpreter,latex,FontName,...
335 Times New Roman,FontSize,fontsize);
35
-
8/21/2019 Project Report Robot Dynamics
37/60
y l a b e l ([FeedForward Torque (Nm) $\rightarrow$],Interpreter,...
latex,...
FontName,Times New Roman,FontSize,fontsize);
l e g e n d ({$\tau_1$,$\tau_2$,$f_3$,$\tau_4$,$\tau_5$,...
340 $\tau_6$},...
Interpreter,latex,FontName,Times New Roman,...
FontSize,12,...Color,none,Location,Best);
345 % Joint Feedback Torques/Forces
i = i+1;
f i g u r e (i);
c l f
p l o t (T, Tau_fd_matrix(1,:),b,T, Tau_fd_matrix(2,:),r,...
350 T, Tau_fd_matrix(3,:),g,T, Tau_fd_matrix(4,:),k,...
T, Tau_fd_matrix(5,:),c,T, Tau_fd_matrix(6,:),m,LineWidth,2);
g r i d on
t i t l e ([Feedback Torque vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
355 x l a b e l (Time $\rightarrow$,Interpreter,latex,FontName,...Times New Roman,FontSize,fontsize);
y l a b e l ([Feedback Torque (Nm) $\rightarrow$],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
l e g e n d ({$\tau_1$,$\tau_2$,$f_3$,$\tau_4$,$\tau_5$,...
360 $\tau_6$},...
Interpreter,latex,FontName,Times New Roman,...
FontSize,12,...
Color,none,Location,Best);
365 % End-effector position
i = i+1;
f i g u r e (i);
c l f
p l o t (T, P0E(:,1),r,T, P0E(:,2),g,T, P0E(:,3),b,LineWidth,2);
370 g r i d on
t i t l e ([End Effector Position vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
x l a b e l (Time $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
375 y l a b e l ([$p0_e$ (m) $\rightarrow$],Interpreter,latex,...
FontName,...
Times New Roman,FontSize,fontsize);
l e g e n d ({$X$,$Y$,$Z$},...
Interpreter,latex,FontName,Times New Roman,...
380
FontSize,12,...Color,none,Location,Best);
t e x t (0.1,0.1,[Radius of minimum size container is ,...
num2str(min_rc)],...
FontName,Times New Roman,FontSize,14);
385
% End-effector position error
36
-
8/21/2019 Project Report Robot Dynamics
38/60
i = i+1;
f i g u r e (i);
c l f
390 p l o t (T, P0E(:,1)-P0E_d(:,1),r,T, P0E(:,2)-P0E_d(:,2),g,...
T, P0E(:,3)-P0E_d(:,3),b,LineWidth,2);
g r i d on
t i t l e ([EE Position Error vs Time],Interpreter,latex,...FontName,Times New Roman,FontSize,fontsize);
395 x l a b e l (Time $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
y l a b e l ([$p0_e$ error (m) $\rightarrow$],Interpreter,latex,...
FontName,...
Times New Roman,FontSize,fontsize);
400 l e g e n d ({$X$,$Y$,$Z$},...
Interpreter,latex,FontName,Times New Roman,...
FontSize,12,...
Color,none,Location,Best);
405 %% Printing the results in pdf files
print_str={theta_1,theta_2,r_3,theta_4,theta_5,theta_6,...
theta_1_dot,theta_2_dot,r_3_dot,theta_4_dot,...
theta_5_dot,...
theta_6_dot,theta_1_e,theta_2_e,r_3_e,theta_4_e,...
410 theta_5_e,theta_6_e,theta_1_dot_e,theta_2_dot_e,...
r_3_dot_e,...
theta_4_dot_e,theta_5_dot_e,theta_6_dot_e,...
energy_diff,energy_balance,energy_error,tau_ff,tau_fd,...
ee_position,ee_pos_e};
415
i f(print_flag)
f o r j=1:i-1
f i g u r e (j+1);
420 export_fig([print_dir prefix print_str{j}],-pdf,...
-transparent);
en d
en d
print_animation_flag = 0;
425 en d
s a v e (datestr(now)); % saving workspace
B MATLAB script for initializing system parameters.% MATLAB Script to initialize the robot parameters
% Constants
g l o b a l g0_bar;
5 g0_bar = [0; 0; 9.81]; % acceleration due to gravity vector
37
-
8/21/2019 Project Report Robot Dynamics
39/60
% Material Properties
rho = 7000; % kg/m 3 (density of link material)
10 % Geometric Parameters
OA = 0.1; % m
AB_1 = 0.1; % mB_2C = 0.1; % m
CE = 0.1; % m
15 D = 0.025*2; % m (outer radius of hollow circular cross-section)
d = 0.02*2; % m (inner radius of hollow circular cross-section)
% D-H Parameters
A = z e r o s (6,1); % robot a_i vector
20 l = OA;
m = CE;
R = [ 0 l 0 0 0 m ] ; % robot r_i vector
Alpha = [ p i /2 p i /2 0 p i /2 p i /2 0]; % robot alpha_i vector
Theta = z e r o s (6,1); % robot theta_i vector
25 P_DH = [A, R, Alpha, Theta]; % System DH parameters
% Inertial Parameters
30 g l o b a l m_OAB_1 m_B_2C m_CE I_OAB_1 I_B_2C I_CE c_OAB_1 c_B_2C c_CE;
% global U_0;
% Mass and inertia calculation for link OAB_1
m_OAB_1 = rho* p i *(D2-d2)*(OA+AB_1)/4; % kg (mass of link OAB_1)
35
FB_1 = D/2+AB_1;
OG = OA-D/2;
m_FB_1 = rho* p i *(D2-d2)*FB_1;
40 m_OG = rho* p i *(D2-d2)*OG;
c_OAB_1 = [0;
(m_FB_1*0+m_OG*(-D-OG)/2)/(m_FB_1+m_OG);
(m_FB_1*(FB_1-D)/2+m_OG*0)/(m_FB_1+m_OG)];
45
% Calculation based inertia
% I_OAB_12_2Y_star = m_FB_1*(3*(D^2+d^2)/4+FB_1^2)/12+...
% m_FB_1*((FB_1-D)/2-c_OAB_1(3))^2+...
% m_OG *(D^2+d^2)/8+m_OG*c_OAB_1(3)^2;
50
%% I_OAB_12_2Z_star = m_FB_1*(D^2+d^2)/8+m_FB_1*c_OAB_1(2)^2+...
% m_OG *(3*(D^2+d^2)/4+OG^2)/12+m_OG*(-D/2-OG/2-c_OAB_1(2))^2;
%
% I_OAB_12_2X_star = I_OAB_12_2Y_star+I_OAB_12_2Z_star;
55 %
% I_OAB_1 = [I_OAB_12_2X_star, 0, 0
38
-
8/21/2019 Project Report Robot Dynamics
40/60
% 0, I_OAB_12_2Y_star, 0;
% 0, 0, I_OAB_12_2Z_star]; %
60 % Solidworks model based inertia
I_OAB_1 = [0.00230683 0.00000000 0.00000000;
0.00000000 0.00140080 0.00062255;
0.00000000 0.00062255 0.00140080];
65 % Mass and inertia calculation for link B_2C
m_B_2C = rho* p i *(D2-d2)*B_2C/4; % kg (mass of link B_2C)
c_B_2C = [0; 0; -B_2C/2];
I_B_2C = [m_B_2C*(3*(D2+d2)/4+B_2C2)/12, 0, 0;
0, m_B_2C*(3*(D2+d2)/4+B_2C2)/12, 0;
70 0, 0, m_B_2C*(D2+d2)/8];
% Mass and inertia calculation for link CE
m_CE = rho* p i *(D2-d2)*CE/4; % kg (mass of link CE)
75 c_CE = [0; 0; -CE/2];
I_CE = [m_CE*(3*(D2+d2)/4+CE2)/12, 0, 0;
0, m_CE*(3*(D2+d2)/4+CE2)/12, 0;
0, 0, m_CE*(D2+d2)/8];
80 % Payload mass for controller
Mpc.mass = 0; % kg payload mass unknown to controller
Mpc. t y p e = point;
Mpc.radius = 0; % m (radius of the spherical payload)
85 % Payload mass for simulator
Mps.mass = 0.4; % kg payload mass unknown to controller
Mps. t y p e = spherical;
% Mps.type = point;
i f(strcmp(Mps. t y pe ,spherical))
90 Mps.radius = 0.025; % m (radius of the spherical payload)
en d
C MATLAB function for robot path planning.
f u n c t i o n [Q_c,Q_i,Q_f] = plan_path(p0_E_i,p0_E_f,Q_dot_i, Q_dot_f,...
Q_ddot_i, Q_ddot_f,ti, tf, P_DH,path_scale)
% Initializing return variable
5 Q_c = z e r o s (6); % coefficient matrix for the polynomials
% Solving for inverse kinematics at initial position
R0_6_i = [1 0 0;0 -1 0;0 0 -1];
Q_all_i = inverse_position_kinematics(p0_E_i, R0_6_i, P_DH);
10 choice = 1;
Q_i = Q_all_i(:,choice);
39
-
8/21/2019 Project Report Robot Dynamics
41/60
% Solving for inverse kinematics at final position
R0_6_f = [1 0 0;0 -1 0;0 0 -1];
15 Q_all_f = inverse_position_kinematics(p0_E_f, R0_6_f, P_DH);
choice = 1;
Q_f = Q_all_f(:,choice);
Q_f = Q_i+path_scale*(Q_f-Q_i);
20
d i s p ([Planning Path from num2str(Q_i) t o num2str(Q_f)]);
A_t = [1 ti ti2 ti3 ti4 ti5;
1 tf tf2 tf3 tf4 tf5;
25 0 1 2*ti 3*ti2 4*ti3 5*ti4;
0 1 2*tf 3*tf2 4*tf3 5*tf4;
0 0 2 6*ti 12*ti2 20*ti3;
0 0 2 6*tf 12*tf2 20*tf3];
inv_A_t = A_t\e y e (6);
30 f o r i=1:6
Q_c(:,i) = inv_A_t*[Q_i(i);Q_f(i);Q_dot_i(i);Q_dot_f(i);Q_ddot_i(i);
Q_ddot_f(i)];
en d
35 d i s p (Done Planning Path!);
D MATLAB function for forward position kinematics.
f u n c t i o n [r0_6,R0_6] = forward_position_kinematics(Q, P_DH)
% MATLAB function to implement the foward position kinematics of the 6 DOF
% Stanford Armr3 = Q(3);
5 A = P_DH(:,1); % robot a_i vector
R = P_DH(:,2); % robot r_i vector
Alpha = P_DH(:,3); % robot alpha_i vector
R(3) = r3;
Theta = [Q(1:2); P_DH(3,4); Q(4:6)]; % robot theta_i vector
10
p6_E = [0 0 0];
A0_6 = A_N_N_1(Theta(1),Alpha(1),A(1),R(1))*...
A_N_N_1(Theta(2),Alpha(2),A(2),R(2))*...
A_N_N_1(Theta(3),Alpha(3),A(3),R(3))*...
15 A_N_N_1(Theta(4),Alpha(4),A(4),R(4))*...
A_N_N_1(Theta(5),Alpha(5),A(5),R(5))*...A_N_N_1(Theta(6),Alpha(6),A(6),R(6));
p0_6 = A0_6*[p6_E; 1];
r0_6 = p0_6(1:3);
20 R0_6 = A0_6(1:3,1:3);
40
-
8/21/2019 Project Report Robot Dynamics
42/60
E MATLAB script for inverse position kinematics.
f u n c t i o n Q_all = inverse_position_kinematics(r0_6, R0_6, P_DH)
% function to evaluate joint position given the position and orientation
% of the end effector
R = P_DH(:,2); % robot r_i vector
5 Alpha = P_DH(:,3); % robot alpha_i vectorm = R(6);
l = R(2);
Q_all = z e r o s (6,8);
10
% Solving for theta_1, theta_2 and r_3
s_6 = [0; 0; m];
q = r0_6-R0_6*s_6;
15 A_t = q(2);
B_t = -q(1);
C_t = l;
[theta_1A, theta_1B] = solve_quadratic(A_t,B_t,C_t);
20
r_3A_plus = s q r t ((q(1)* c o s (theta_1A)+q(2)* s i n (theta_1A))2+q(3)2);
r_3A_minus = - s q r t ((q(1)* c o s (theta_1A)+q(2)* s i n (theta_1A))2+q(3)2);
r_3B_plus = s q r t ((q(1)* c o s (theta_1B)+q(2)* s i n (theta_1B))2+q(3)2);
25 r_3B_minus = - s q r t ((q(1)* c o s (theta_1B)+q(2)* s i n (theta_1B))2+q(3)2);
i f(a bs(r_3A_plus)>10(-6))
sin_theta_2A_plus = (q(1)* c o s (theta_1A)+q(2)* s i n (theta_1A))/r_3A_plus;
cos_theta_2A_plus = q(3)/(-r_3A_plus);
30
theta_2A_plus = a t a n2 (sin_theta_2A_plus,cos_theta_2A_plus);e l s e
theta_2A_plus = 0;
en d
35 i f(a bs(r_3A_minus)>10(-6))
sin_theta_2A_minus = (q(1)* c o s (theta_1A)+q(2)* s i n (theta_1A))/r_3A_minus;
cos_theta_2A_minus = q(3)/(-r_3A_minus);
theta_2A_minus = a t a n2 (sin_theta_2A_minus,cos_theta_2A_minus);
e l s e
40 theta_2A_minus = 0;
en d
i f(a bs(r_3B_plus)>10(-6))sin_theta_2B_plus = (q(1)* c o s (theta_1B)+q(2)* s i n (theta_1B))/r_3B_plus;
45 cos_theta_2B_plus = q(3)/(-r_3B_plus);
theta_2B_plus = a t a n2 (sin_theta_2B_plus,cos_theta_2B_plus);
e l s e
theta_2B_plus = 0;
en d
41
-
8/21/2019 Project Report Robot Dynamics
43/60
50
i f(a bs(r_3B_minus)>10(-6))
sin_theta_2B_minus = (q(1)* c o s (theta_1B)+q(2)* s i n (theta_1B))/r_3B_minus;
cos_theta_2B_minus = q(3)/(-r_3B_minus);
theta_2B_minus = a t a n2 (sin_theta_2B_minus,cos_theta_2B_minus);
55 e l s e
theta_2B_minus = 0;en d
Q_all(1:3,1:4) = [theta_1A, theta_1A, theta_1B, theta_1B;
60 theta_2A_plus, theta_2A_minus, theta_2B_plus, theta_2B_minus;
r_3A_plus, r_3A_minus, r_3B_plus, r_3B_minus];
Q_all(1:3,5:8) = [theta_1A, theta_1A, theta_1B, theta_1B;
theta_2A_plus, theta_2A_minus, theta_2B_plus, theta_2B_minus;
65 r_3A_plus, r_3A_minus, r_3B_plus, r_3B_minus];
J = V_matrix(Alpha(1));
% Solving for theta_4 - theta_670 f o r i=1:4
U_1 = U_matrix(Q_all(1,i));
U_2 = U_matrix(Q_all(2,i));
P = J *U_2*J*U_1*R0_6;
75 A_t = P(2,3);
B_t = -P(1,3);
C_t = 0;
[theta_4A, theta_4B] = solve_quadratic(A_t,B_t,C_t);
80 theta_6A = a t a n2 (P(1,1)* s i n (theta_4A)-P(2,1)* c o s (theta_4A),...
P(1,2)* s i n (theta_4A)-P(2,2)* c o s (theta_4A));
theta_6B = a t a n2 (P(1,1)* s i n (theta_4B)-P(2,1)* c o s (theta_4B),...
P(1,2)* s i n (theta_4B)-P(2,2)* c o s (theta_4B));
85 theta_5A = a t a n2 (P(1,3)* c o s (theta_4A)+P(2,3)* s i n (theta_4A),...
-P(3,3));
theta_5B = a t a n2 (P(1,3)* c o s (theta_4B)+P(2,3)* s i n (theta_4B),...
-P(3,3));
Q_all(4,i) = theta_4A;
90 Q_all(4,i+4) = theta_4B;
Q_all(5,i) = theta_5A;
Q_all(5,i+4) = theta_5B;
Q_all(6,i) = theta_6A;
Q_all(6,i+4) = theta_6B;95
% Equation Checks
i f( abs( c o s (theta_5A)* c o s (theta_6A)-P(1,1)* c o s (theta_4A)-P(2,1)*...
s i n (theta_4A))>10(-10))
d i s p (Inverse Position Kinematics: Check 1 Failed);
100 pa us e;
42
-
8/21/2019 Project Report Robot Dynamics
44/60
en d
i f( abs( c o s (theta_5A)* s i n (theta_6A)+P(1,2)* c o s (theta_4A)+P(2,2)*...
s i n (theta_4A))>10(-10))
105 d i s p (Inverse Position Kinematics: Check 2 Failed);
pa us e;
en d
i f( abs( s i n (theta_5A)* c o s (theta_6A)-P(3,1))>10(-10))
110 d i s p (Inverse Position Kinematics: Check 3 Failed);
pa us e;
en d
i f( abs( s i n (theta_5A)* s i n (theta_6A)+P(3,2))>10(-10))
115 d i s p (Inverse Position Kinematics: Check 4 Failed);
pa us e;
en d
en d
F MATLAB function for inverse dynamics.
f u n c t i o n [Tau,s0_i_hat,I0_i_hat] = inverse_dynamics(Q,Q_dot,Q_ddot,...
P_DH,Mp)
% MATLAB function to implement the inverse dynamics of the 6 DOF Stanford
% Arm
5 %Input variables
% Q - current position of the joints
% Q_dot - current velocity of the joints
% Q_ddot - required acceleration of the joints
% P_DH - DH parameters of the manipulator10 % Mp - Payload mass details
%% error/warning handling
d bs to p i f warning
d bs to p i f e r ro r
15 %% main code
g l o b a l m_OAB_1 m_B_2C m_CE I_OAB_1 I_B_2C I_CE c_OAB_1 c_B_2C c_CE g0_bar;
Ci_i = z e r o s (3,6);
20 Ci_i(:,2) = c_OAB_1;
Ci_i(:,3) = c_B_2C;
Ci_i(:,6) = m_CE*c_CE/(m_CE+Mp.mass);
r3 = Q(3);
25 A = P_DH(:,1); % robot a_i vector
R = P_DH(:,2); % robot r_i vector
Alpha = P_DH(:,3); % robot alpha_i vector
R(3) = r3;
43
-
8/21/2019 Project Report Robot Dynamics
45/60
Theta = [Q(1:2); P_DH(3,4); Q(4:6)]; % robot theta_i vector
30
%% Computing s0_i_hat and inertia I0_i_hat
A0_i = z e r o s (4,4,6);
s0_i_hat = z e r o s (6,6);
35 I0_i_hat = z e r o s (6,6,6);m_i(1) = 0;
m_i(2) = m_OAB_1;
m_i(3) = m_B_2C;
m_i(4) = 0;
40 m_i(5) = 0;
m_i(6) = m_CE;
I_i_star(:,:,1) = z e r o s (3);
I_i_star(:,:,2) = I_OAB_1;
I_i_star(:,:,3) = I_B_2C;
45 I_i_star(:,:,4) = z e r o s (3);
I_i_star(:,:,5) = z e r o s (3);
i f(strcmp(Mp. t y pe ,point))
I_Mp = [m_CE*(c_CE(3)-Ci_i(3,6))2+Mp.mass*Ci_i(3,6)2, 0, 0;
0, m_CE*(c_CE(3)-Ci_i(3,6))2+Mp.mass*Ci_i(3,6)2, 0;
50 0, 0, 0];
e l s e i f(strcmp(Mp. type ,spherical))
I_Mp=[m_CE*(c_CE(3)-Ci_i(3,6))2+2*Mp.mass*Mp.radius2/5+...
Mp.mass*Ci_i(3,6)2, 0, 0;
0, m_CE*(c_CE(3)-Ci_i(3,6))2+2*Mp.mass*Mp.radius2/5+...
55 Mp.mass*Ci_i(3,6)2, 0;
0, 0, 0];
e l s e
e r r o r(Mass type not supported!);
en d
60
I_i_star(:,:,6) = I_CE+I_Mp;
f o r i=1:6
i f(i==1)
65 A0_i(:,:,i) = A_N_N_1(Theta(i),Alpha(i),A(i),R(i));
e l s e
A0_i(:,:,i) = A0_i(:,:,i-1)*A_N_N_1(Theta(i),Alpha(i),A(i),R(i));
en d
i f(i==1)
70 s0_i_hat(:,i) = [0;0;1; z e r o s (3,1)];
e l s e i f(i==3)
s0_i_hat(:,i) = [ z e r o s (3,1);A0_i(1:3,3,i-1)];
e l s e s0_i_hat(:,i) = [A0_i(1:3,3,i-1); c r o s s (A0_i(1:3,4,i-1),...
75 A0_i(1:3,3,i-1))];
en d
c0_i = A0_i(:,:,i)*[Ci_i(:,i);1];
C0_i(:,i) = c0_i(1:3);
44
-
8/21/2019 Project Report Robot Dynamics
46/60
80 X0_i_star_hat = [A0_i(1:3,1:3,i), z e r o s (3);
cross_form(C0_i(:,i))*A0_i(1:3,1:3,i), A0_i(1:3,1:3,i)];
Ii_star_i_hat = [ z e r o s (3), m_i(i)* e y e (3);
I_i_star(:,:,i), z e r o s (3)];
I0_i_hat(:,:,i) = X0_i_star_hat*Ii_star_i_hat*...
85 spatial_transpose(X0_i_star_hat);
%% Checks
% If the eigen values of the rotation matrix are not 1
i f( abs(max( abs( e i g (A0_i(1:3,1:3,i))))-1)>10E-9)
90 warning(Inverse Dynamics -> Rotation matrix invalid!);
en d
en d
95
%% Kinematic recursion
% spatial velocity of links (considering virtual links)
V0_i_hat = z e r o s (6,6);
% spatial acceleration of links (considering virtual links)100 A0_i_hat = z e r o s (6,6);
v0_0_hat = z e r o s (6,1); % spatial velocity of base
a0_0_hat = [ z e r o s (3,1);g0_bar]; % spatial acceleration of base
f o r i=1:6
i f(i==1)
105 V0_i_hat(:,i) = v0_0_hat + s0_i_hat(:,i)*Q_dot(i);
A0_i_hat(:,i) = a0_0_hat +...
spatial_cross(V0_i_hat(:,i),s0_i_hat(:,i)*Q_dot(i))+...
s0_i_hat(:,i)*Q_ddot(i);
e l s e
110 V0_i_hat(:,i) = V0_i_hat(:,i-1) + s0_i_hat(:,i)*Q_dot(i);
A0_i_hat(:,i) = A0_i_hat(:,i-1) +...
spatial_cross(V0_i_hat(:,i),s0_i_hat(:,i)*Q_dot(i))+...
s0_i_hat(:,i)*Q_ddot(i);
en d
115 en d
%% Force recursion
f0_i_hat = z e r o s (6);
f0_e_hat = z e r o s (6,1);
120 Tau = z e r o s (6,1);
f o r i=6:-1:1
i f(i==6)
f0_i_hat(:,i) = -f0_e_hat+I0_i_hat(:,:,i)*A0_i_hat(:,i)+...
spatial_cross(V0_i_hat(:,i),I0_i_hat(:,:,i)*V0_i_hat(:,i));125 e l s e
f0_i_hat(:,i) = f0_i_hat(:,i+1)+I0_i_hat(:,:,i)*A0_i_hat(:,i)+...
spatial_cross(V0_i_hat(:,i),I0_i_hat(:,:,i)*V0_i_hat(:,i));
en d
130 Tau(i) = spatial_transpose(s0_i_hat(:,i))*f0_i_hat(:,i);
45
-
8/21/2019 Project Report Robot Dynamics
47/60
en d
G MATLAB function for forward dynamics.
f u n c t i o n Q_ddot = forward_dynamics(Tau,Q,Q_dot,P_DH, Mp)
% MATLAB Function to evaluate the forward dynamics of the stanford
% manipulator
5 %% error/warning handling
d bs to p i f warning
d bs to p i f e r ro r
%% main code
10 [b,s0_i_hat,I0_i_hat] = inverse_dynamics(Q,Q_dot,z e r o s (6,1),P_DH, Mp);
I0c_i_hat = z e r o s (6,6,6); % composite spatial inertia matrix
M_matrix = z e r o s
(6,6); % mass matrixf o r i=6:-1:1
15 i f(i==6)
I0c_i_hat(:,:,i) = I0_i_hat(:,:,i);
e l s e
I0c_i_hat(:,:,i) = I0c_i_hat(:,:,i+1)+I0_i_hat(:,:,i);
en d
20
% I matrix symmetry check wrt spatial transpose
i f(max(max( abs(I0c_i_hat(:,:,i)-...
spatial_transpose(I0c_i_hat(:,:,i)))))>10E-10)
warning([Forward Dynamics -> I matrix not symmetric,...
25 wrt Spatial Transpose!]);
pa us e;en d
en d
30
f o r i=1:6
f o r j=1:i
M_matrix(i,j) = spatial_transpose(s0_i_hat(:,j))*...
I0c_i_hat(:,:,max(i,j))*s0_i_hat(:,i);
35 M_matrix(j,i) = M_matrix(i,j);
en d
en d
40 [V D ] = e i g (M_matrix);
i f(min( d i a g (D))
-
8/21/2019 Project Report Robot Dynamics
48/60
Q_ddot(6) = 0;
e l s e
Q_ddot = i n v (M_matrix)*(Tau-b);
50 % M_matrix symmetry check
i f(max(max( abs(M_matrix-M_matrix)))>10E-10)
warning(Forward Dynamics -> M matrix not symmetric!);pa us e;
en d
55 en d
H MATLAB function to simulate manipulator dynamics for
ode5.
f u n c t i o n [X_dot] = manipulator_dynamics(t,X,P_DH,Mp,Tau)
%% Function to simulate the forward dynamics of the manipulator
% Counter to just show time with increase of 1 sec
5 persistent counter;
i f(t==0)
counter = 0;
en d
10
Q = X(1:6);
Q_dot = X(7:12);
i f(t>counter)
15 t
counter = counter+1;
en d
% Evaluating joint accelerations for the evaluated torque
20 Q_ddot = forward_dynamics(Tau,Q,Q_dot,P_DH,Mp);
X_dot(1:6) = Q_dot;
X_dot(7:12) = Q_ddot;
X_dot = X_dot(:);
25 % Check
i f(max( abs(X_dot))>1)
warning(Large X_dot);
en d
I MATLAB function for visualizing stanford arm configura-
tion.
47
-
8/21/2019 Project Report Robot Dynamics
49/60
f u n c t i o n plot_stanford_arm(Q, P_DH)
% Function to draw a 3D visualization of the given configuration of the
% Stanford Arm
frame_scale = 0.05; % scale for plotting coordinate frames
5
r3 = Q(3);A = P_DH(:,1); % robot a_i vector
R = P_DH(:,2); % robot r_i vector
R(3) = r3;
10 Alpha = P_DH(:,3); % robot alpha_i vector
Theta = [Q(1:2); P_DH(3,4); Q(4:6)]; % robot theta_i vector
p0_O = [0 0 0]; % Location of point O in frame 0
p1_A = [0 0 R(2)]; % Location of point A in frame 1
15 % 2p_B = [0 0 ];
p2_C = [0 0 r3]; % Location of point C in frame 2
% p5_E = [0 0 R(6)]; % Location of point E in frame 5
p0_A = A_N_N_1(Theta(1),Alpha(1),A(1),R(1))*[p1_A; 1];20 p0_C = A_N_N_1(Theta(1),Alpha(1),A(1),R(1))*...
A_N_N_1(Theta(2),Alpha(2),A(2),R(2))*[p2_C; 1];
[p0_E, R0_6] = forward_position_kinematics(Q, P_DH);
% Homogenous transform of frame i in frame 0
25 A0 = e y e (4);
A0_i(:,:,1) = A_N_N_1(Theta(1),Alpha(1),A(1),R(1));
A0_i(:,:,2) = A0_i(:,:,1)*...
A_N_N_1(Theta(2),Alpha(2),A(2),R(2));
A0_i(:,:,3) = A0_i(:,:,2)*...
30 A_N_N_1(Theta(3),Alpha(3),A(3),R(3));
A0_i(:,:,4) = A0_i(:,:,3)*...
A_N_N_1(Theta(4),Alpha(4),A(4),R(4));A0_i(:,:,5) = A0_i(:,:,4)*...
A_N_N_1(Theta(5),Alpha(5),A(5),R(5));
35 A0_i(:,:,6) = A0_i(:,:,5)*...
A_N_N_1(Theta(6),Alpha(6),A(6),R(6));
%% Plotting Stanford Arm
f i g u r e (1);
40 c l f;
% Plotting Frames
color = {-r;-g;-b};
f o r i=1:3
45 p l o t 3 ([0 A0(1,i)]*frame_scale,[0 A0(2,i)]*frame_scale,...
[0 A0(3,i)]*frame_scale,cell2mat(color(i)),LineWidth,2);
ho l d on;
en d
50 f o r j=1:6
f o r i=1:3
48
-
8/21/2019 Project Report Robot Dynamics
50/60
p l o t 3 (A0_i(1,4,j)+[0 A0_i(1,i,j)]*frame_scale,...
A0_i(2,4,j)+[0 A0_i(2,i,j)]*frame_scale,...
A0_i(3,4,j)+[0 A0_i(3,i,j)]*frame_scale,cell2mat(color(i)),...
55 LineWidth,2);
en d
en d
% Plotting Actual Robot
60 p l o t 3 ([p0_O(1) p0_A(1)],[p0_O(2) p0_A(2)], [p0_O(3) p0_A(3)],...
-b,LineWidth,4);
p l o t 3 ([p0_A(1) p0_C(1)],[p0_A(2) p0_C(2)], [p0_A(3) p0_C(3)],...
-r,LineWidth,4);
p l o t 3 ([p0_C(1) p0_E(1)],[p0_C(2) p0_E(2)], [p0_C(3) p0_E(3)],...
65 -k,LineWidth,4);
a x i s ([-0.5 0.5 -0.5 0.5 -0.5 0.5 0 1]);
% axis vis3d;
g r i d on;
t i t l e (Stanford Arm Configuration,Interpreter,latex,...
70 FontName,Times New Roman,FontSize,14);
x l a b e l (X0 $\rightarrow$,Interpreter,latex,FontName,...Times New Roman,FontSize,14);
y l a b e l (Y0 $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,14);
75 z l a b e l (Z0 $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,14);
drawnow;
J MATLAB function for calculating mass matrix.
f u n c t i o n [I0_i_hat,I0c_i_hat,M_matrix,m_i,s0_i_hat,C0_i] = ...
calculate_mass_matrix(Q,P_DH,Mp)
% Function to evaluate the mass matrix for the given payload mass
%Input variables
5 % Q - current position of the joints
% P_DH - DH parameters of the manipulator
% Mp - Payload mass details
%% error/warning handling
10 d b st op i f warning
d bs to p i f e r ro r
%% main codeg l o b a l m_OAB_1 m_B_2C m_CE I_OAB_1 I_B_2C I_CE c_OAB_1 c_B_2C c_CE;
15
Ci_i = z e r o s (3,6);
C0_i = z e r o s (3,6);
Ci_i(:,2) = c_OAB_1;
49
-
8/21/2019 Project Report Robot Dynamics
51/60
20 Ci_i(:,3) = c_B_2C;
Ci_i(:,6) = m_CE*c_CE/(m_CE+Mp.mass);
r3 = Q(3);
A = P_DH(:,1); % robot a_i vector
25 R = P_DH(:,2); % robot r_i vector
Alpha = P_DH(:,3); % robot alpha_i vectorR(3) = r3;
Theta = [Q(1:2); P_DH(3,4); Q(4:6)]; % robot theta_i vector
30 %% Computing s0_i_hat and inertia I0_i_hat
A0_i = z e r o s (4,4,6);
s0_i_hat = z e r o s (6,6);
I0_i_hat = z e r o s (6,6,6);
35 m_i(1) = 0;
m_i(2) = m_OAB_1;
m_i(3) = m_B_2C;
m_i(4) = 0;
m_i(5) = 0;
40 m_i(6) = m_CE;
I_i_star(:,:,1) = z e r o s (3);
I_i_star(:,:,2) = I_OAB_1;
I_i_star(:,:,3) = I_B_2C;
I_i_star(:,:,4) = z e r o s (3);
45 I_i_star(:,:,5) = z e r o s (3);
i f(strcmp(Mp. t y pe ,point))
I_Mp = [m_CE*(c_CE(3)-Ci_i(3,6))2+Mp.mass*Ci_i(3,6)2, 0, 0;
0, m_CE*(c_CE(3)-Ci_i(3,6))2+Mp.mass*Ci_i(3,6)2, 0;
50 0, 0, 0];
e l s e i f(strcmp(Mp. type ,spherical))
I_Mp=[m_CE*(c_CE(3)-Ci_i(3,6))2+2*Mp.mass*Mp.radius2/5+...
Mp.mass*Ci_i(3,6)2, 0, 0;
0, m_CE*(c_CE(3)-Ci_i(3,6))2+2*Mp.mass*Mp.radius2/5+...
55 Mp.mass*Ci_i(3,6)2, 0;
0, 0, 0];
e l s e
e r r o r(Mass type not supported!);
en d
60
I_i_star(:,:,6) = I_CE+I_Mp;
f o r i=1:6
i f(i==1)65 A0_i(:,:,i) = A_N_N_1(Theta(i),Alpha(i),A(i),R(i));
e l s e
A0_i(:,:,i) = A0_i(:,:,i-1)*A_N_N_1(Theta(i),Alpha(i),A(i),R(i));
en d
i f(i==1)
70 s0_i_hat(:,i) = [0;0;1; z e r o s (3,1)];
50
-
8/21/2019 Project Report Robot Dynamics
52/60
e l s e i f(i==3)
s0_i_hat(:,i) = [ z e r o s (3,1);A0_i(1:3,3,i-1)];
e l s e
s0_i_hat(:,i) = [A0_i(1:3,3,i-1); c r o s s (A0_i(1:3,4,i-1),...
75 A0_i(1:3,3,i-1))];
en d
c0_i = A0_i(:,:,i)*[Ci_i(:,i);1];
C0_i(:,i) = c0_i(1:3);
80 X0_i_star_hat = [A0_i(1:3,1:3,i), z e r o s (3);
cross_form(C0_i(:,i))*A0_i(1:3,1:3,i), A0_i(1:3,1:3,i)];
Ii_star_i_hat = [ z e r o s (3), m_i(i)* e y e (3);
I_i_star(:,:,i), z e r o s (3)];
I0_i_hat(:,:,i) = X0_i_star_hat*Ii_star_i_hat*...
85 spatial_transpose(X0_i_star_hat);
%% Checks
% If the eigen values of the rotation matrix are not 1
i f( abs(max( abs( e i g (A0_i(1:3,1:3,i))))-1)>10E-9)
90 warning(Inverse Dynamics -> Rotation matrix invalid!);
en d
en d
95 I0c_i_hat = z e r o s (6,6,6); % composite spatial inertia matrix
M_matrix = z e r o s (6,6); % mass matrix
f o r i=6:-1:1
i f(i==6)
I0c_i_hat(:,:,i) = I0_i_hat(:,:,i);
100 e l s e
I0c_i_hat(:,:,i) = I0c_i_hat(:,:,i+1)+I0_i_hat(:,:,i);
en d
% I matrix symmetry check wrt spatial transpose
105 i f(max(max( abs(I0c_i_hat(:,:,i)-...
spatial_transpose(I0c_i_hat(:,:,i)))))>10E-10)
warning([Forward Dynamics -> I matrix not symmetric,...
wrt Spatial Transpose!]);
pa us e;
110 en d
en d
f o r i=1:6
115
f o r j=1:iM_matrix(i,j) = spatial_transpose(s0_i_hat(:,j))*...
I0c_i_hat(:,:,max(i,j))*s0_i_hat(:,i);
M_matrix(j,i) = M_matrix(i,j);
en d
120 en d
51
-
8/21/2019 Project Report Robot Dynamics
53/60
K MATLAB function for calculating energy.
f u n c t i o n [ke,pe,ae] = calculate_energy(Q,Q_dot,P_DH,Mp,dts,Tau)
g l o b a l g0_bar;
5 ke = 0;pe = 0;
ae = 0;
[I0_i_hat,,,m_i,s0_i_hat,C0_i] = calculate_mass_matrix(Q,P_DH,Mp);
10
%% Kinematic recursion
% spatial velocity of links (considering virtual links)
V0_i_hat = z e r o s (6,6);
% spatial acceleration of links (considering virtual links)
15 A0_i_hat = z e r o s (6,6);
v0_0_hat = z e r o s (6,1); % spatial velocity of base
a0_0_hat = [ z e r o s (3,1);g0_bar]; % spatial acceleration of base
f o r i=1:6
i f(i==1)
20 V0_i_hat(:,i) = v0_0_hat + s0_i_hat(:,i)*Q_dot(i);
e l s e
V0_i_hat(:,i) = V0_i_hat(:,i-1) + s0_i_hat(:,i)*Q_dot(i);
en d
25 ke = ke + 0.5*spatial_transpose(V0_i_hat(:,i))*I0_i_hat(:,:,i)*...
V0_i_hat(:,i);
pe = pe + m_i(i)*g0_bar(3)*C0_i(3,i);
en d
30
ae = Tau*Q_dot*dts;
en d
L MATLAB function for expressingNth coordinate frame in
terms of(N1)th coordinate frame using homogeneous trans-
formation.
f u n c t i o n A = A_N_N_1(theta, alpha ,a,r)
% Homogenous transform from frame N to frame N-1A = [ c o s (theta), - s i n (theta)* c o s ( a l pha ), s i n (theta)* s i n ( a l pha ),...
a* c o s (theta);
5 s i n (theta), c o s (theta)* c o s ( a l pha ), - c o s (theta)* s i n ( a l pha ),...
a* s i n (theta);
0, s i n ( a l pha ), c o s ( a l pha ), r;
0, 0, 0, 1];
52
-
8/21/2019 Project Report Robot Dynamics
54/60
M MATLAB function for solving quadratic equation.
f u n c t i o n [theta_1A, theta_1B] = solve_quadratic(A_t,B_t,C_t)
i f(a bs(C_t-A_t)>10(-6)) % If C_t!= A_t
theta_1A = 2* a t a n ((-B_t+ s q r t (B_t2-C_t2+A_t2))/(C_t-A_t));
theta_1B = 2* a t a n ((-B_t- s q r t (B_t2-C_t2+A_t2))/(C_t-A_t));
5 e l s e i f( abs(B_t)>10(-6))% Check whether this is correct when C=A
theta_1A = 2* a t a n (-(A_t+C_t)/(2*B_t));
theta_1B = theta_1A;
e l s e
10 % Check whether this is correct when C=A
theta_1A = 2* a t a n (A_t+C_t);
theta_1B = -theta_1A;
en d
N MATLAB function for evaluating spatial cross product.f u n c t i o n A_hat_cross_hat_B_hat = spatial_cross(A_hat,B_hat)
A_bar = A_hat(1:3);
A0_bar = A_hat(4:6);
A_hat_cross_hat = [cross_form(A_bar) z e r o s (3);
5 cross_form(A0_bar) cross_form(A_bar)];
A_hat_cross_hat_B_hat = A_hat_cross_hat*B_hat;
O MATLAB function for evaluating spatial transpose.
f u n c t i o n X_s = spatial_transpose(X)
i f( s i z e (X,2)==1)
X_s = [X(4:6) X(1:3)];
e l s e
5 A = X(1:3,1:3);
B = X(1:3,4:6);
C = X(4:6,1:3);
D = X(4:6,4:6);
X_s = [D B;
10 C A];
en d
P MATLAB function for evaluating vectorsi.
f u n c t i o n s = s_vector(i,P_DH)
A = P_DH(:,1); % robot a_i vector
R = P_DH(:,2); % robot r_i vector
53
-
8/21/2019 Project Report Robot Dynamics
55/60
s = [A(i) 0 R(i)];
Q MATLAB function for evaluating matrixU.
f u n c t i o n U = U_matrix(theta)
U = [ c o s (theta) - s i n (theta) 0; s i n (theta) c o s (theta) 0; 0 0 1];
R MATLAB function for evaluating matrixV.
f u n c t i o n V = V_matrix(a l pha )
V = [ 1 0 0 ; 0 c o s ( a l pha ) - s i n ( a l pha ); 0 s i n ( a l pha ) c o s ( a l pha )];
S MATLAB function for expressing a spatial vector in crossform.
f u n c t i o n c_cross = cross_form(c)
c_cross = [0 -c(3) c(2);
c(3) 0 -c(1);
-c(2) c(1) 0];
T MATLAB function for testing inverse and forward position
kinematics.
% MATLAB Code for testing the position kinematics using random inputs
% AUTHOR : Priyanshu Agarwal
%% INITIAL CLEANING
% ==============================
5 c l e a r ;
c l c ;
%% Figure print setup
c l e a r
10
% User-defined Parametersprint_flag = 1; % all plots will be printed as pdf if 1
% (only valid if animation_flag = 1)
15 i f print_flag
addpath(/home/priyanshu/Dropbox/MATLAB/export_fig);
54
-
8/21/2019 Project Report Robot Dynamics
56/60
% Directory to print the pdf files
print_dir = [/home/priyanshu/Dropbox/UT_Courses/2013_Spring/,...
20 Advanced_Dynamics_of_Robotic_Systems/Project/Project_Report/figures];
fontsize = 18;
e l s e
fontsize = 14;
en d25
%% INITIALISATION AND PARAMETER DEFINITION:
% =========================================
% System parameter initialization
30 system_parameters_plant;
n = 100;
Q = z e r o s (6,1);
Q_d = z e r o s (6,n); % desired Q
Q_e = z e r o s (6,n); % evaluated Q
35
f o r i=1:n
Q = 3*(rand(6,1)-0.5);
[r0_6, R0_6] = forward_position_kinematics(Q, P_DH);
40 Q_all = inverse_position_kinematics(r0_6, R0_6, P_DH);
% plot_stanford_arm([Q_all(:,3); Q(4:6)],P_DH);
Q_diff = abs (Q_all-repmat(Q,1,8));
[C,I] = min(mean(Q_diff),[],2);
Q_d(:,i) = Q;
45 Q_e(:,i) = Q_all(:,I);
max(C)
i f((min(mean(abs(Q_all-repmat(Q,1,8))))>0.0000001) ||...
max(max( i s n a n (Q_all))))
pa us e;
50 en d
en d
str = {$\theta_1$,$\theta_2$,$r_3$,...
$\theta_4$,$\theta_5$,$\theta_6$,...
55 $\theta_{1e}$,$\theta_{2e}$,$r_{3e}$,...
$\theta_{4e}$,$\theta_{5e}$,$\theta_{6e}$};
strd = {$\theta_{1d}$,$\theta_{2d}$,$r_{3d}$,...
$\theta_{4d}$,$\theta_{5d}$,$\theta_{6d}$};
60
f o r i=1:6
f i g u r e (i);c l f
p l o t (1:n,Q_d(i,:),-b,1:n,Q_e(i,:),-r);
65 g r i d on
t i t l e ([str{i} vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
x l a b e l (Time $\rightarrow$,Interpreter,latex,FontName,...
55
-
8/21/2019 Project Report Robot Dynamics
57/60
Times New Roman,FontSize,fontsize);
70 y l a b e l ([str{i} $\rightarrow$],Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
l e g e n d ({str{i},strd{i}},...
Interpreter,latex,FontName,Times New Roman,FontSize,12,...
Color,none,Location,Best);
75 en d
f o r i=1:6
f i g u r e (i+6);
c l f
80 p l o t (1:n,Q_d(i,:)-Q_e(i,:),-b);
g r i d on
t i t l e ([str{i+6} vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
x l a b e l (Time $\rightarrow$,Interpreter,latex,FontName,...
85 Times New Roman,FontSize,fontsize);
y l a b e l ([str{i+6} $\rightarrow$],Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
en d
90
%% Printing the results in pdf files
print_str={theta_1,theta_2,r_3,theta_4,theta_5,theta_6,...
theta_1_e,theta_2_e,r_3_e,theta_4_e,...
theta_5_e,theta_6_e};
95
i f print_flag
prefix = /test_position_kinematics;
en d
100 i f(print_flag)
f