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 specically, we develop algorithms for path planning and PD
control of the simulated manipulator (plant) (running at 1000Hz) with the end-effector payload
mass (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
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 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
4.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 conguration. 47
J MATLAB function for calculating mass matrix. 49
1
K MATLAB function for calculating energy. 52
L MATLAB function for expressing N
th
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 vector s
i
. 53
Q MATLAB function for evaluating matrix U. 54
R MATLAB function for evaluating matrix V . 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
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
Task Planning
System Parameter
Initialization
(Module:
system_parameters_plant)
ODE Solver (dt !"!!# se$)
(Module: ode% & manipulator_dynami$s)
Plan Pat'
(inverse kinemati$s)
(Module: plan_pat')
Mass matri(
s$aled PID
gains
(Module: main)
Task Spe$i)i$ations
(Module: main)
System Simulation *oop (dt !"!# se$)
&
&
Plant
+
&
,
))
,
)d
-ontroller
.$tual
/inemati$s
Plot and Print
0esults
(Module: main)
Simulate Planned Pat'
()or1ard kinemati$s)
(Module: main)
-al$ulate Mass
Matri( (unkno1n
payload)
(Module:
$al$ulate_mass_matri()
Inverse Dynami$s
(Module: inverse_dynami$s)
/inemati$ 0e$ursion & 2or$e 0e$ursion
2or1ard Dynami$s (kno1n payload)
(Module: )or1ard_dynami$s)
Inverse Dynami$s
(Module: inverse_dynami$s)
Desired
/inemati$s
/inemati$ and Inertial Parameters
/inemati$ and Inertial Parameters
Figure 1: Overall structure of the program along with the major modules implemented mentioned
in the corresponding block.
Figure 1 presents 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 dened along with their function are as follows:
1. Main: This is the main module integrating all other modules to simulate the overall system.
(Appendix A)
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. (Appendix B)
3
3. Path Planning: This module is used to plan the trajectories for the various joints of the
manipulator given the initial and nal conguration of the end-effector using polynomial
interpolation. (Appendix C)
4. Forward Position Kinematics: This module is used to implement the forward position kine-
matics of the manipulator. (Appendix D)
5. Inverse Position Kinematics: This module is used to implement the inverse position kine-
matics of the manipulator. (Appendix E)
6. Inverse Dynamics: This module is used to implement the inverse dynamics of the manipula-
tor. (Appendix F)
7. Forward Dynamics: This moduel is used to implement the forward dynamics of the manip-
ulator. (Appendix G)
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. (Appendix H)
9. Plot Stanford Arm: This module is implemented to plot a 3D visual of the given congura-
tion of the manipulator.(Appendix I)
10. Calculate Mass Matrix: This module is implemented to calculate the mass matrix of the
system. (Appendix J)
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. (Appendix K)
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 both
the coordinate axes orientation and origin location. (Appendix L)
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. (Appendix O)
16. S Vector: This module is implemented to evaluate the vectors s
i
s needed for inverse dynam-
ics. (Appendix P)
17. U matrix: This module is implemented to evaluate the U matrix for coordinate transforma-
tion. (Appendix Q)
4
18. V matrix: This module is implemented to evaluate the V matrix for coordinate transforma-
tion. (Appendix R)
19. Cross Form: This module is developed to express a vector in its cross form. (Appendix S)
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. (Appendix T)
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. (Appendix U)
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 Chart
Figure 2 presents the ow 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 nal conguration 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 the
controller time step and checked against the nal time. If the time variable is less than the nal
simulation time the loop continues, otherwise, the loop ends. Finally, the results are plotted and
the simulation stops.
5
Initialize System
Parameters
Stop
Plan Path using inverse
kinematics
t<tf
Evaluate controller torque with unknown
payload mass (feedforward + feedack!
t " current time
tf " simulation final time
dtc " controller time step
dts " simulator time step
Simulate #orward $ynamics with known
payload mass for dtc time with a time step
of dts
%alculate kinetic& potential&
and actuator energy
Plot results
'es
(o
t ) t + dtc
(i)
Evaluate desired joint
positions and velocities
at time t
Calculate Mass Matrix
(unknown payload)
Calculate feedforward torque
using inverse dynamics
Calculate feedback torque using
error, ! gains and mass matrix
(ii)
Initialize joint position,
velocity states and evaluate
torque input at time t
Use ode5 (fixed time-step
solver) to set up integration for
time dtc wit a time-step of dts
!imulate forward dynamics
of te system wit te "nown
payload mass
!tore te ode5 results in
te system state varia#le
(iii)
Figure 2: Flow chart of the program along with the ow chart of the sub-modules implemented.
(i) Overall ow chart, (ii) Controller ow chart, (iii) Plant ow chart.
6
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.
Figure 3 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. Figure 4 presents the
error in the results from forward and inverse position kinematics. Again, the errors are very small
of the order of 10
13
10
16
which exist due to machine precision. This the simulation shows
both 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, rst, 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.
Figure 5 presents 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. Figure 6 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
0 10 20 30 40 50 60 70 80 90 100
1.5
1
0.5
0
0.5
1
1.5

1
vs Time
Time

1d
(i)
0 10 20 30 40 50 60 70 80 90 100
1.5
1
0.5
0
0.5
1
1.5

2
vs Time
Time

2d
(ii)
0 10 20 30 40 50 60 70 80 90 100
1.5
1
0.5
0
0.5
1
1.5
r
3
vs Time
Time
r
3

r
3
r
3d
(iii)
0 10 20 30 40 50 60 70 80 90 100
1.5
1
0.5
0
0.5
1
1.5

4
vs Time
Time

4d
(iv)
0 10 20 30 40 50 60 70 80 90 100
1.5
1
0.5
0
0.5
1
1.5

5
vs Time
Time

5d
(v)
0 10 20 30 40 50 60 70 80 90 100
1.5
1
0.5
0
0.5
1
1.5

6
vs Time
Time

6d
(vi)
Figure 3: Forward-inverse position kinematics for 100 random inputs. Generated using MATLAB
script in Appendix T.
.
8
0 10 20 30 40 50 60 70 80 90 100
0.5
0
0.5
1
1.5
2
2.5
3
x 10
13

1e
vs Time
Time

1
e

(i)
0 10 20 30 40 50 60 70 80 90 100
0.5
0
0.5
1
1.5
2
2.5
3
x 10
14

2e
vs Time
Time

2
e

(ii)
0 10 20 30 40 50 60 70 80 90 100
6
4
2
0
2
4
6
8
10
x 10
16
r
3e
vs Time
Time
r
3
e

(iii)
0 10 20 30 40 50 60 70 80 90 100
0.5
0
0.5
1
1.5
2
2.5
3
x 10
13

4e
vs Time
Time

4
e

(iv)
0 10 20 30 40 50 60 70 80 90 100
0.5
0
0.5
1
1.5
2
2.5
3
x 10
14

5e
vs Time
Time

5
e

(v)
0 10 20 30 40 50 60 70 80 90 100
2
1
0
1
2
3
4
5
6
x 10
14

6e
vs Time
Time

6
e

(vi)
Figure 4: Forward-inverse position kinematics error for 100 random inputs. Generated using MAT-
LAB script in Appendix T.
9
0 1 2 3 4 5 6 7 8 9 10
0.2
0.15
0.1
0.05
0
0.05
0.1
0.15
0.2

1
vs Time
Time

1d
(i)
0 1 2 3 4 5 6 7 8 9 10
0.03
0.02
0.01
0
0.01
0.02
0.03

2
vs Time
Time

2d
(ii)
0 1 2 3 4 5 6 7 8 9 10
2.5
2
1.5
1
0.5
0
0.5
1
1.5
2
2.5
x 10
3
r
3
vs Time
Time
r
3

r
3
r
3d
(iii)
0 1 2 3 4 5 6 7 8 9 10
4
3
2
1
0
1
2
3
x 10
13

4
vs Time
Time

4d
(iv)
0 1 2 3 4 5 6 7 8 9 10
0.03
0.02
0.01
0
0.01
0.02
0.03

5
vs Time
Time

5d
(v)
0 1 2 3 4 5 6 7 8 9 10
0.2
0.15
0.1
0.05
0
0.05
0.1
0.15
0.2

6
vs Time
Time

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 Appendix U.
. 10
0 1 2 3 4 5 6 7 8 9 10
2
1.5
1
0.5
0
0.5
1
1.5
2
2.5
x 10
14

1e
vs Time
Time

1
e

(i)
0 1 2 3 4 5 6 7 8 9 10
1.5
1
0.5
0
0.5
1
1.5
x 10
14

2e
vs Time
Time

2
e

(ii)
0 1 2 3 4 5 6 7 8 9 10
3
2
1
0
1
2
x 10
14
r
3e
vs Time
Time
r
3
e

(iii)
0 1 2 3 4 5 6 7 8 9 10
4
3
2
1
0
1
2
3
x 10
13

4e
vs Time
Time

4
e

(iv)
0 1 2 3 4 5 6 7 8 9 10
6
4
2
0
2
4
6
8
x 10
13

5e
vs Time
Time

5
e

(v)
0 1 2 3 4 5 6 7 8 9 10
10
8
6
4
2
0
2
4
6
8
x 10
14

6e
vs Time
Time

6
e

(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 Appendix U.
.
11
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-
ing 1) 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.
(Appendix E).
cos
5
cos
6
= p
11
cos
4
p
21
sin
4
(1)
Listing 1: Code snippet to check for the various equations.
% Equation Checks
i f (abs(cos(theta_5A)
*
cos(theta_6A)-P(1,1)
*
cos (theta_4A)-P(2,1)
*
...
s i n(theta_4A))>10(-10))
di s p(Inverse Position Kinematics: Check 1 Failed);
5 pause;
end
i f (abs(cos(theta_5A)
*
s i n(theta_6A)+P(1,2)
*
cos(theta_4A)+P(2,2)
*
...
s i n(theta_4A))>10(-10))
10 di s p(Inverse Position Kinematics: Check 2 Failed);
pause;
end
i f (abs(s i n(theta_5A)
*
cos(theta_6A)-P(3,1))>10(-10))
15 di s p(Inverse Position Kinematics: Check 3 Failed);
pause;
end
i f (abs(s i n(theta_5A)
*
s i n(theta_6A)+P(3,2))>10(-10))
20 di s p(Inverse Position Kinematics: Check 4 Failed);
pause;
end
2. We also used the check of inertia tensor being symmetric with respect to the spatial transpose
(Listing 2) and mass matrix with respect to the matrix transpose (Listing 2) i.e.
I
S
= I (2)
M
T
= 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
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!]);
pause;
end
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!);
pause;
5 end
3. Furthermore, we used the check on the eigenvalues of the rotation matrix all of which should
have 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!);
end
13
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) +

t
0
()
T

()d (4)
U(t) =
6

j=1
1
2
v
s
j

I
j
v
j
+
6

j=1
m
j
gc
jz
(5)
Also,
dU(t) = ()
T

()d (6)
0 2 4 6 8 10
0.5
0
0.5
1
1.5
2
2.5
3
3.5
4
x 10
4
Instantaneous Energy Dierence vs Time
Time
E
n
e
r
g
y
(
J
o
u
l
e
s
)

Total Energy Di
Instantaneous Actuator Energy Input
(i)
0 2 4 6 8 10
0
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2
Energy Check vs Time
Time
E
n
e
r
g
y
(
J
o
u
l
e
s
)

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
0 2 4 6 8 10
2
1.5
1
0.5
0
0.5
1
x 10
4
Energy Error vs Time
Time
E
n
e
r
g
y
E
r
r
o
r
(
J
o
u
l
e
s
)

(iii)
Figure 7: Energy differential, total system and actuator energy, and energy error plots for 10 second
simulation of the planned path.
Figure 7 presents the results of the energy check carried out for the 10 second simulation for
the planned path. Figure 7i plots both the sides of Eqn. 6 and Figure 7ii plots both the sides of
Eqn. 4. As can be seen the two sides of both the equations match. Figure 7iii shows 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 (Listing 5) 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 (Appendix H).
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);
end
2. We also used the MATLAB in-build error and warning handling (Listing 6) to check for the
source of the error and warning for debugging and initiate automatic enabling of break point
15
at any such occurrence (Appendix A).
Listing 6: Code snippet to enable MATLAB in-build error and warning handling.
%% Error/Warning Handling
dbst op i f warning
dbst op i f error
16
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
Figure 8 presents the conguration 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. Figures 11 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.
Figure 13 presents 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 nal 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 10
8
m.
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 (Figure 14). Note the change in the order of the error
magnitude with the variation in the payload mass.
17
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0
Stanford Arm Conguration
Y0
t = 0 sec
Z
0

(i)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0
Stanford Arm Conguration
Y0
t = 1 sec
Z
0

(ii)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0
Stanford Arm Conguration
Y0
t = 2 sec
Z
0

(iii)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0
Stanford Arm Conguration
Y0
t = 3 sec
Z
0

(iv)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0
Stanford Arm Conguration
Y0
t = 4 sec
Z
0

(v)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0
Stanford Arm Conguration
Y0
t = 5 sec
Z
0

(vi)
Figure 8: Stanford arm conguration at various time instants during the path tracking task. The
time for each conguration is mentioned in the gure. (contd.)
18
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0
Stanford Arm Conguration
Y0
t = 6 sec
Z
0

(vii)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0
Stanford Arm Conguration
Y0
t = 7 sec
Z
0

(viii)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0
Stanford Arm Conguration
Y0
t = 8 sec
Z
0

(ix)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0
Stanford Arm Conguration
Y0
t = 9 sec
Z
0

(x)
0.5
0
0.5
0.5
0
0.5
0.5
0
0.5
X0
Stanford Arm Conguration
Y0
t = 10 sec
Z
0

(xi)
Figure 8: Stanford arm conguration at various time instants during the path tracking task. The
time for each conguration is mentioned in the gure.
19
0 1 2 3 4 5 6 7 8 9 10
0.5
0
0.5
1
1.5
2
2.5
3

1
vs Time
Time

1d
(i)
0 1 2 3 4 5 6 7 8 9 10
2.05
2
1.95
1.9
1.85
1.8
1.75
1.7
1.65
1.6
1.55

2
vs Time
Time

2d
(ii)
0 1 2 3 4 5 6 7 8 9 10
0.445
0.45
0.455
0.46
0.465
0.47
0.475
0.48
0.485
0.49
r
3
vs Time
Time
r
3

r
3
r
3d
(iii)
0 1 2 3 4 5 6 7 8 9 10
3
2
1
0
1
2
3
4
x 10
4

4
vs Time
Time

4d
(iv)
0 1 2 3 4 5 6 7 8 9 10
1.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

5d
(v)
0 1 2 3 4 5 6 7 8 9 10
0.5
0
0.5
1
1.5
2
2.5
3

6
vs Time
Time

6d
(vi)
Figure 9: Position tracking results with payload mass of 0.4 kg.
20
0 1 2 3 4 5 6 7 8 9 10
0.7
0.6
0.5
0.4
0.3
0.2
0.1
0
0.1

1
vs Time
Time

1d
(i)
0 1 2 3 4 5 6 7 8 9 10
0.09
0.08
0.07
0.06
0.05
0.04
0.03
0.02
0.01
0
0.01

2
vs Time
Time

2d
(ii)
0 1 2 3 4 5 6 7 8 9 10
9
8
7
6
5
4
3
2
1
0
1
x 10
3
r
3
vs Time
Time

r
3

r
3
r
3d
(iii)
0 1 2 3 4 5 6 7 8 9 10
2
1
0
1
2
3
4
x 10
4

4
vs Time
Time

4d
(iv)
0 1 2 3 4 5 6 7 8 9 10
0.09
0.08
0.07
0.06
0.05
0.04
0.03
0.02
0.01
0
0.01

5
vs Time
Time

5d
(v)
0 1 2 3 4 5 6 7 8 9 10
0.1
0
0.1
0.2
0.3
0.4
0.5
0.6

6
vs Time
Time

6d
(vi)
Figure 10: Velocity tracking results for payload mass of 0.4 kg.
21
0 1 2 3 4 5 6 7 8 9 10
2
1.5
1
0.5
0
0.5
1
1.5
x 10
5

1e
vs Time
Time

1
e

(i)
0 1 2 3 4 5 6 7 8 9 10
10
8
6
4
2
0
2
x 10
6

2e
vs Time
Time

2
e

(ii)
0 1 2 3 4 5 6 7 8 9 10
16
14
12
10
8
6
4
2
0
2
x 10
6
r
3e
vs Time
Time
r
3
e

(iii)
0 1 2 3 4 5 6 7 8 9 10
3
2
1
0
1
2
3
4
x 10
4

4e
vs Time
Time

4
e

(iv)
0 1 2 3 4 5 6 7 8 9 10
1
0
1
2
3
4
5
6
7
x 10
4

5e
vs Time
Time

5
e

(v)
0 1 2 3 4 5 6 7 8 9 10
10
8
6
4
2
0
2
x 10
5

6e
vs Time
Time

6
e

(vi)
Figure 11: Position tracking error results with payload mass of 0.4 kg.
22
0 1 2 3 4 5 6 7 8 9 10
0.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 10
0.09
0.08
0.07
0.06
0.05
0.04
0.03
0.02
0.01
0
0.01

2e
vs Time
Time

2
e

(ii)
0 1 2 3 4 5 6 7 8 9 10
9
8
7
6
5
4
3
2
1
0
1
x 10
3
r
3e
vs Time
Time

r
3
e

(iii)
0 1 2 3 4 5 6 7 8 9 10
4
3
2
1
0
1
2
x 10
4

4e
vs Time
Time

4
e

(iv)
0 1 2 3 4 5 6 7 8 9 10
0.09
0.08
0.07
0.06
0.05
0.04
0.03
0.02
0.01
0
0.01

5e
vs Time
Time

5
e

(v)
0 1 2 3 4 5 6 7 8 9 10
0.1
0
0.1
0.2
0.3
0.4
0.5
0.6

6e
vs Time
Time

6
e

(vi)
Figure 12: Velocity tracking error results with payload mass of 0.4 kg.
23
0 2 4 6 8 10
5
4
3
2
1
0
1
2
3
4
5
FeedForward Torque vs Time
Time
F
e
e
d
F
o
r
w
a
r
d
T
o
r
q
u
e
(
N
m
)

2
f
3

6
(i)
0 2 4 6 8 10
3
2
1
0
1
2
3
4
5
x 10
3
Feedback Torque vs Time
Time
F
e
e
d
b
a
c
k
T
o
r
q
u
e
(
N
m
)

2
f
3

6
(ii)
0 1 2 3 4 5 6 7 8 9 10
0.5
0.4
0.3
0.2
0.1
0
0.1
0.2
0.3
0.4
0.5
End Eector Position vs Time
Time
p
0e
(
m
)

Radius of minimum size container is 2.8465e07
X
Y
Z
(iii)
0 1 2 3 4 5 6 7 8 9 10
4
3
2
1
0
1
2
3
x 10
5
EE Position Error vs Time
Time
p
0e
e
r
r
o
r
(
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
0 1 2 3 4 5 6 7 8 9 10
8
6
4
2
0
2
4
6
x 10
7
EE Position Error vs Time
Time
p
0e
e
r
r
o
r
(
m
)

X
Y
Z
(i) payload mass = 0 gm
0 2 4 6 8 10
2
1.5
1
0.5
0
0.5
1
1.5
x 10
5
EE Position Error vs Time
Time
p
0e
e
r
r
o
r
(
m
)

X
Y
Z
(ii) payload mass = 100 gm
0 2 4 6 8 10
3
2.5
2
1.5
1
0.5
0
0.5
1
1.5
2
x 10
5
EE Position Error vs Time
Time
p
0e
e
r
r
o
r
(
m
)

X
Y
Z
(iii) payload mass = 200 gm
0 1 2 3 4 5 6 7 8 9 10
4
3
2
1
0
1
2
3
x 10
5
EE Position Error vs Time
Time
p
0e
e
r
r
o
r
(
m
)

X
Y
Z
(iv) payload mass = 300 gm
0 1 2 3 4 5 6 7 8 9 10
4
3
2
1
0
1
2
3
x 10
5
EE Position Error vs Time
Time
p
0e
e
r
r
o
r
(
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
4.2 Problem 2
Once you have performed the inverse position kinematics at the initial and the nal posi-
tions, obtain a new nal position that results from reducing the angular displacements at
each joint to 10% of its original values. Next, assume that the payload of unknown mass M is
a 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
= M

K
p

Q
d
Q

+K
v

Q
d

(7)
K
p
= 3556 I
66
(8)
K
v
= 101 I
66
(9)
0 1 2 3 4 5 6 7 8 9 10
0.2
0.1
0
0.1
0.2
0.3
0.4
0.5
End Eector Position vs Time
Time
p
0e
(
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.474210
8
m.
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
(Figure 16). Here also the results show the same trend as discussed in Section 4.1.1.
26
0 1 2 3 4 5 6 7 8 9 10
2
1
0
1
2
3
4
5
6
7
x 10
8
EE Position Error vs Time
Time
p
0e
e
r
r
o
r
(
m
)

X
Y
Z
(i) payload mass = 0 gm
0 1 2 3 4 5 6 7 8 9 10
8
6
4
2
0
2
4
6
8
x 10
7
EE Position Error vs Time
Time
p
0e
e
r
r
o
r
(
m
)

X
Y
Z
(ii) payload mass = 100 gm
0 2 4 6 8 10
1.5
1
0.5
0
0.5
1
1.5
x 10
6
EE Position Error vs Time
Time
p
0e
e
r
r
o
r
(
m
)

X
Y
Z
(iii) payload mass = 200 gm
0 2 4 6 8 10
2
1.5
1
0.5
0
0.5
1
1.5
2
x 10
6
EE Position Error vs Time
Time
p
0e
e
r
r
o
r
(
m
)

X
Y
Z
(iv) payload mass = 300 gm
0 2 4 6 8 10
2
1.5
1
0.5
0
0.5
1
1.5
2
x 10
6
EE Position Error vs Time
Time
p
0e
e
r
r
o
r
(
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
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 ngered gripper as the three ngers 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 might
itself 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).

M
11
O
M
21
1

1
0

b
1
+M
12

2
b
2
+M
22

(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
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. In IEEE 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 unc t i on 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 ar ;
15 c l c ;
%% Error/Warning Handling
dbst op i f warning
dbst op i f error
20
%% Figure print setup
c l e ar
% 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
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;
end
%% 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
*
eye(6);
50 K_v = 101
*
eye(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 = zeros (6,1);
Q_dot_i = zeros (6,1);
Q_ddot_i = zeros (6,1);
65
p0_E_f = [-0.4; 0.1; 0.1];
Q_f = zeros (6,1);
Q_dot_f = zeros (6,1);
Q_ddot_f = zeros (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 or t=ti:0.1:tf
30
Q_d = Q_c
*
[1;t;t2;t3;t4;t5];
plot_stanford_arm(Q_d,P_DH);
pause(0.01);
end
85 end
tstart = ti;
tend = tf;
90 T = [tstart:dts:tend];
%% Actual Simulation
% Changing payload mass
f or 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 end
X = zeros (l e ngt h(T),12);
Tau_ff_matrix = zeros (6,l e ngt h(T));
Tau_fd_matrix = zeros (6,l e ngt h(T));
105 KE = zeros (s i z e (T));
PE = zeros (s i z e (T));
AE = zeros (s i z e (T));
P0E = zeros (l e ngt h(T),3);
P0E_d = zeros (l e ngt 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);
end
f or 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);
end
130 % Evaluating desired joint angles, velocities and accelerations
Q_d = Q_c
*
[1;t;t2;t3;t4;t5];
31
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(end),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(end))) sec],...
150 FontName,Times New Roman,FontSize,14);
export_fig([print_dir prefix config,...
num2str(round(T_s(end)
*
fc))],...
-pdf,-transparent);
end
155
X(round(t
*
fs)+1:round(t
*
fs)+l e ngt h(T_s),:) = X_s;
Tau_ff_matrix(:,round(t
*
fs)+1:round(t
*
fs)+l e ngt h(T_s)) =...
repmat(Tau_ff,1,11);
Tau_fd_matrix(:,round(t
*
fs)+1:round(t
*
fs)+l e ngt h(T_s)) =...
160 repmat(Tau_fd,1,11);
end
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
*
...
[zeros (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 or 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
P0E(i,:) = r0_6;
[r0_6_d,] = forward_position_kinematics(Q_d(:,i), P_DH);
185 P0E_d(i,:) = r0_6_d;
end
% [r0_6_d,~] = forward_position_kinematics(Q_d(:,end), P_DH);
190
min_rc = norm(r0_6(1:2)-r0_6_d(1:2));
di 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 or i=1:6
f i gur e (i+1);
c l f
pl ot (T, X(1:l e ngt h(T),i),-b,T, Q_d(i,:),-r,LineWidth,2);
gri d on
215 t i t l e ([str{i} vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
xl abe l (Time $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
yl abe l ([str{i} $\rightarrow$],Interpreter,latex,...
220 FontName,...
Times New Roman,FontSize,fontsize);
l egend({str{i},strd{i}},...
Interpreter,latex,FontName,Times New Roman,...
FontSize,12,...
225 Color,none,Location,Best);
end
% theta_dot
f or i=1:6
230 f i gur e (i+7);
c l f
pl ot (T, X(1:l e ngt h(T),i+6),-b,T, Q_dot_d(i,:),-r,...
LineWidth,2);
33
gri d on
235 t i t l e ([str{i+6} vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
xl abe l (Time $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
yl abe l ([str{i+6} $\rightarrow$],Interpreter,latex,...
240 FontName,Times New Roman,FontSize,fontsize);
l egend({str{i+6},strd{i+6}},...
Interpreter,latex,FontName,Times New Roman,...
FontSize,12,...
Color,none,Location,Best);
245 end
% theta_error
f or i=1:6
f i gur e (i+13);
250 c l f
pl ot (T, X(1:l e ngt h(T),i)-Q_d(i,:),-b,LineWidth,2);
gri d on
t i t l e ([str{i+12} vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
255 xl abe l (Time $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
yl abe l ([str{i+12} $\rightarrow$],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
end
260
% theta_dot_error
f or i=1:6
f i gur e (i+19);
c l f
265 pl ot (T, X(1:l e ngt h(T),i+6)-Q_dot(i,:),-b,LineWidth,2);
gri d on
t i t l e ([str{i+18} vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
xl abe l (Time $\rightarrow$,Interpreter,latex,FontName,...
270 Times New Roman,FontSize,fontsize);
yl abe l ([str{i+18} $\rightarrow$],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
end
275 % Energy difference
i = i+20;
f i gur e (i);
c l f
pl ot (T(2:end), d i f f (KE)+d i f f (PE),-b,T,AE,-k,LineWidth,2);
280 gri d on
t i t l e ([Instantaneous Energy Difference vs Time],Interpreter,...
latex,...
FontName,Times New Roman,FontSize,fontsize);
xl abe l (Time $\rightarrow$,Interpreter,latex,FontName,...
34
285 Times New Roman,FontSize,fontsize);
yl abe l ([Energy (Joules) $\rightarrow$],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
l egend({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 gur e (i);
c l f
pl ot (T, KE+PE-KE(1)-PE(1),-b,T,cumsum(AE),-k,LineWidth,2);
gri d on
t i t l e ([Energy Check vs Time],Interpreter,latex,...
300 FontName,Times New Roman,FontSize,fontsize);
xl abe l (Time $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
yl abe l ([Energy (Joules) $\rightarrow$],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
305 l egend({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 gur e (i);
c l f
pl ot (T, KE+PE-KE(1)-PE(1)-cumsum(AE),-b,LineWidth,2);
315 gri d on
t i t l e ([Energy Error vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
xl abe l (Time $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
320 yl abe l ([Energy Error (Joules) $\rightarrow$],Interpreter,...
latex,...
FontName,Times New Roman,FontSize,fontsize);
% Joint Feedforward Torques/Forces
325 i = i+1;
f i gur e (i);
c l f
pl ot (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);
gri d on
t i t l e ([FeedForward Torque vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
xl abe l (Time $\rightarrow$,Interpreter,latex,FontName,...
335 Times New Roman,FontSize,fontsize);
35
yl abe l ([FeedForward Torque (Nm) $\rightarrow$],Interpreter,...
latex,...
FontName,Times New Roman,FontSize,fontsize);
l egend({$\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 gur e (i);
c l f
pl ot (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);
gri d on
t i t l e ([Feedback Torque vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
355 xl abe l (Time $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
yl abe l ([Feedback Torque (Nm) $\rightarrow$],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
l egend({$\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 gur e (i);
c l f
pl ot (T, P0E(:,1),r,T, P0E(:,2),g,T, P0E(:,3),b,LineWidth,2);
370 gri d on
t i t l e ([End Effector Position vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
xl abe l (Time $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
375 yl abe l ([$p0_e$ (m) $\rightarrow$],Interpreter,latex,...
FontName,...
Times New Roman,FontSize,fontsize);
l egend({$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
i = i+1;
f i gur e (i);
c l f
390 pl ot (T, P0E(:,1)-P0E_d(:,1),r,T, P0E(:,2)-P0E_d(:,2),g,...
T, P0E(:,3)-P0E_d(:,3),b,LineWidth,2);
gri d on
t i t l e ([EE Position Error vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
395 xl abe l (Time $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
yl abe l ([$p0_e$ error (m) $\rightarrow$],Interpreter,latex,...
FontName,...
Times New Roman,FontSize,fontsize);
400 l egend({$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 or j=1:i-1
f i gur e (j+1);
420 export_fig([print_dir prefix print_str{j}],-pdf,...
-transparent);
end
end
print_animation_flag = 0;
425 end
save(datestr(now)); % saving workspace
B MATLAB script for initializing system parameters.
% MATLAB Script to initialize the robot parameters
% Constants
gl obal g0_bar;
5 g0_bar = [0; 0; 9.81]; % acceleration due to gravity vector
37
% Material Properties
rho = 7000; % kg/m^3 (density of link material)
10 % Geometric Parameters
OA = 0.1; % m
AB_1 = 0.1; % m
B_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 = zeros (6,1); % robot a_i vector
20 l = OA;
m = CE;
R = [0 l 0 0 0 m]; % robot r_i vector
Alpha = [pi /2 pi /2 0 pi /2 pi /2 0]; % robot alpha_i vector
Theta = zeros (6,1); % robot theta_i vector
25 P_DH = [A, R, Alpha, Theta]; % System DH parameters
% Inertial Parameters
30 gl obal 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
*
pi
*
(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
*
pi
*
(D2-d2)
*
FB_1;
40 m_OG = rho
*
pi
*
(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
% 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
*
pi
*
(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
*
pi
*
(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 ype = 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 ype = spherical;
% Mps.type = point;
i f (strcmp(Mps.type ,spherical))
90 Mps.radius = 0.025; % m (radius of the spherical payload)
end
C MATLAB function for robot path planning.
f unc t i on [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 = zeros (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
% 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
di s p([Planning Path from num2str(Q_i) to 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\eye(6);
30 f or 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)];
end
35 di s p(Done Planning Path!);
D MATLAB function for forward position kinematics.
f unc t i on [r0_6,R0_6] = forward_position_kinematics(Q, P_DH)
% MATLAB function to implement the foward position kinematics of the 6 DOF
% Stanford Arm
r3 = 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
E MATLAB script for inverse position kinematics.
f unc t i on 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 vector
m = R(6);
l = R(2);
Q_all = zeros (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 qr t ((q(1)
*
cos(theta_1A)+q(2)
*
s i n(theta_1A))2+q(3)2);
r_3A_minus = -s qr t ((q(1)
*
cos(theta_1A)+q(2)
*
s i n(theta_1A))2+q(3)2);
r_3B_plus = s qr t ((q(1)
*
cos(theta_1B)+q(2)
*
s i n(theta_1B))2+q(3)2);
25 r_3B_minus = -s qr t ((q(1)
*
cos(theta_1B)+q(2)
*
s i n(theta_1B))2+q(3)2);
i f (abs(r_3A_plus)>10(-6))
sin_theta_2A_plus = (q(1)
*
cos (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 = at an2(sin_theta_2A_plus,cos_theta_2A_plus);
e l s e
theta_2A_plus = 0;
end
35 i f (abs(r_3A_minus)>10(-6))
sin_theta_2A_minus = (q(1)
*
cos(theta_1A)+q(2)
*
s i n(theta_1A))/r_3A_minus;
cos_theta_2A_minus = q(3)/(-r_3A_minus);
theta_2A_minus = at an2(sin_theta_2A_minus,cos_theta_2A_minus);
e l s e
40 theta_2A_minus = 0;
end
i f (abs(r_3B_plus)>10(-6))
sin_theta_2B_plus = (q(1)
*
cos (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 = at an2(sin_theta_2B_plus,cos_theta_2B_plus);
e l s e
theta_2B_plus = 0;
end
41
50
i f (abs(r_3B_minus)>10(-6))
sin_theta_2B_minus = (q(1)
*
cos(theta_1B)+q(2)
*
s i n(theta_1B))/r_3B_minus;
cos_theta_2B_minus = q(3)/(-r_3B_minus);
theta_2B_minus = at an2(sin_theta_2B_minus,cos_theta_2B_minus);
55 e l s e
theta_2B_minus = 0;
end
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_6
70 f or 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 = at an2(P(1,1)
*
s i n(theta_4A)-P(2,1)
*
cos (theta_4A),...
P(1,2)
*
s i n(theta_4A)-P(2,2)
*
cos(theta_4A));
theta_6B = at an2(P(1,1)
*
s i n(theta_4B)-P(2,1)
*
cos (theta_4B),...
P(1,2)
*
s i n(theta_4B)-P(2,2)
*
cos(theta_4B));
85 theta_5A = at an2(P(1,3)
*
cos(theta_4A)+P(2,3)
*
s i n(theta_4A),...
-P(3,3));
theta_5B = at an2(P(1,3)
*
cos(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(cos(theta_5A)
*
cos(theta_6A)-P(1,1)
*
cos (theta_4A)-P(2,1)
*
...
s i n(theta_4A))>10(-10))
di s p(Inverse Position Kinematics: Check 1 Failed);
100 pause;
42
end
i f (abs(cos(theta_5A)
*
s i n(theta_6A)+P(1,2)
*
cos(theta_4A)+P(2,2)
*
...
s i n(theta_4A))>10(-10))
105 di s p(Inverse Position Kinematics: Check 2 Failed);
pause;
end
i f (abs(s i n(theta_5A)
*
cos(theta_6A)-P(3,1))>10(-10))
110 di s p(Inverse Position Kinematics: Check 3 Failed);
pause;
end
i f (abs(s i n(theta_5A)
*
s i n(theta_6A)+P(3,2))>10(-10))
115 di s p(Inverse Position Kinematics: Check 4 Failed);
pause;
end
end
F MATLAB function for inverse dynamics.
f unc t i on [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 manipulator
10 % Mp - Payload mass details
%% error/warning handling
dbst op i f warning
dbst op i f error
15 %% main code
gl obal 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 = zeros (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
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 = zeros (4,4,6);
s0_i_hat = zeros (6,6);
35 I0_i_hat = zeros (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) = zeros (3);
I_i_star(:,:,2) = I_OAB_1;
I_i_star(:,:,3) = I_B_2C;
45 I_i_star(:,:,4) = zeros (3);
I_i_star(:,:,5) = zeros (3);
i f (strcmp(Mp.type ,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
error(Mass type not supported!);
end
60
I_i_star(:,:,6) = I_CE+I_Mp;
f or 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));
end
i f (i==1)
70 s0_i_hat(:,i) = [0;0;1;zeros (3,1)];
e l s e i f (i==3)
s0_i_hat(:,i) = [zeros (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 os s (A0_i(1:3,4,i-1),...
75 A0_i(1:3,3,i-1))];
end
c0_i = A0_i(:,:,i)
*
[Ci_i(:,i);1];
C0_i(:,i) = c0_i(1:3);
44
80 X0_i_star_hat = [A0_i(1:3,1:3,i), zeros (3);
cross_form(C0_i(:,i))
*
A0_i(1:3,1:3,i), A0_i(1:3,1:3,i)];
Ii_star_i_hat = [zeros (3), m_i(i)
*
eye(3);
I_i_star(:,:,i), zeros (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!);
end
end
95
%% Kinematic recursion
% spatial velocity of links (considering virtual links)
V0_i_hat = zeros (6,6);
% spatial acceleration of links (considering virtual links)
100 A0_i_hat = zeros (6,6);
v0_0_hat = zeros (6,1); % spatial velocity of base
a0_0_hat = [zeros (3,1);g0_bar]; % spatial acceleration of base
f or 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);
end
115 end
%% Force recursion
f0_i_hat = zeros (6);
f0_e_hat = zeros (6,1);
120 Tau = zeros (6,1);
f or 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));
end
130 Tau(i) = spatial_transpose(s0_i_hat(:,i))
*
f0_i_hat(:,i);
45
end
G MATLAB function for forward dynamics.
f unc t i on 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
dbst op i f warning
dbst op i f error
%% main code
10 [b,s0_i_hat,I0_i_hat] = inverse_dynamics(Q,Q_dot,zeros (6,1),P_DH, Mp);
I0c_i_hat = zeros (6,6,6); % composite spatial inertia matrix
M_matrix = zeros (6,6); % mass matrix
f or 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);
end
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!]);
pause;
end
end
30
f or i=1:6
f or 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);
end
end
40 [V D] = e i g(M_matrix);
i f (min(di ag(D))<10e-6) % Check if the robot is in a singular configuration
M_matrix(:,end)=[];
M_matrix(end,:)=[];
45 Q_ddot = pi nv(M_matrix)
*
(Tau(1:5)-b(1:5));
46
Q_ddot(6) = 0;
e l s e
Q_ddot = i nv(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!);
pause;
end
55 end
H MATLAB function to simulate manipulator dynamics for
ode5.
f unc t i on [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;
end
10
Q = X(1:6);
Q_dot = X(7:12);
i f (t>counter)
15 t
counter = counter+1;
end
% 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);
end
I MATLAB function for visualizing stanford arm congura-
tion.
47
f unc t i on 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 = eye(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 gur e (1);
40 c l f ;
% Plotting Frames
color = {-r;-g;-b};
f or i=1:3
45 pl ot 3([0 A0(1,i)]
*
frame_scale,[0 A0(2,i)]
*
frame_scale,...
[0 A0(3,i)]
*
frame_scale,cell2mat(color(i)),LineWidth,2);
hol d on;
end
50 f or j=1:6
f or i=1:3
48
pl ot 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);
end
end
% Plotting Actual Robot
60 pl ot 3([p0_O(1) p0_A(1)],[p0_O(2) p0_A(2)], [p0_O(3) p0_A(3)],...
-b,LineWidth,4);
pl ot 3([p0_A(1) p0_C(1)],[p0_A(2) p0_C(2)], [p0_A(3) p0_C(3)],...
-r,LineWidth,4);
pl ot 3([p0_C(1) p0_E(1)],[p0_C(2) p0_E(2)], [p0_C(3) p0_E(3)],...
65 -k,LineWidth,4);
axi s ([-0.5 0.5 -0.5 0.5 -0.5 0.5 0 1]);
% axis vis3d;
gri d on;
t i t l e (Stanford Arm Configuration,Interpreter,latex,...
70 FontName,Times New Roman,FontSize,14);
xl abe l (X0 $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,14);
yl abe l (Y0 $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,14);
75 z l a be l (Z0 $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,14);
drawnow;
J MATLAB function for calculating mass matrix.
f unc t i on [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 dbst op i f warning
dbst op i f error
%% main code
gl obal 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 = zeros (3,6);
C0_i = zeros (3,6);
Ci_i(:,2) = c_OAB_1;
49
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 vector
R(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 = zeros (4,4,6);
s0_i_hat = zeros (6,6);
I0_i_hat = zeros (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) = zeros (3);
I_i_star(:,:,2) = I_OAB_1;
I_i_star(:,:,3) = I_B_2C;
I_i_star(:,:,4) = zeros (3);
45 I_i_star(:,:,5) = zeros (3);
i f (strcmp(Mp.type ,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
error(Mass type not supported!);
end
60
I_i_star(:,:,6) = I_CE+I_Mp;
f or 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));
end
i f (i==1)
70 s0_i_hat(:,i) = [0;0;1;zeros (3,1)];
50
e l s e i f (i==3)
s0_i_hat(:,i) = [zeros (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 os s (A0_i(1:3,4,i-1),...
75 A0_i(1:3,3,i-1))];
end
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), zeros (3);
cross_form(C0_i(:,i))
*
A0_i(1:3,1:3,i), A0_i(1:3,1:3,i)];
Ii_star_i_hat = [zeros (3), m_i(i)
*
eye(3);
I_i_star(:,:,i), zeros (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!);
end
end
95 I0c_i_hat = zeros (6,6,6); % composite spatial inertia matrix
M_matrix = zeros (6,6); % mass matrix
f or 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);
end
% 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!]);
pause;
110 end
end
f or i=1:6
115 f or j=1:i
M_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);
end
120 end
51
K MATLAB function for calculating energy.
f unc t i on [ke,pe,ae] = calculate_energy(Q,Q_dot,P_DH,Mp,dts,Tau)
gl obal 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 = zeros (6,6);
% spatial acceleration of links (considering virtual links)
15 A0_i_hat = zeros (6,6);
v0_0_hat = zeros (6,1); % spatial velocity of base
a0_0_hat = [zeros (3,1);g0_bar]; % spatial acceleration of base
f or 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);
end
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);
end
30 ae = Tau
*
Q_dot
*
dts;
end
L MATLAB function for expressing N
th
coordinate frame in
terms of (N1)
th
coordinate frame using homogeneous trans-
formation.
f unc t i on A = A_N_N_1(theta,al pha ,a,r)
% Homogenous transform from frame N to frame N-1
A = [cos(theta), -s i n(theta)
*
cos(al pha), s i n(theta)
*
s i n(al pha),...
a
*
cos(theta);
5 s i n(theta), cos(theta)
*
cos(al pha), -cos (theta)
*
s i n(al pha),...
a
*
s i n(theta);
0, s i n(al pha), cos(al pha), r;
0, 0, 0, 1];
52
M MATLAB function for solving quadratic equation.
f unc t i on [theta_1A, theta_1B] = solve_quadratic(A_t,B_t,C_t)
i f (abs(C_t-A_t)>10(-6)) % If C_t!= A_t
theta_1A = 2
*
at an((-B_t+s qr t (B_t2-C_t2+A_t2))/(C_t-A_t));
theta_1B = 2
*
at an((-B_t-s qr 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
*
at an(-(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
*
at an(A_t+C_t);
theta_1B = -theta_1A;
end
N MATLAB function for evaluating spatial cross product.
f unc t i on 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) zeros (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 unc t i on 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];
end
P MATLAB function for evaluating vector s
i
.
f unc t i on s = s_vector(i,P_DH)
A = P_DH(:,1); % robot a_i vector
R = P_DH(:,2); % robot r_i vector
53
s = [A(i) 0 R(i)];
Q MATLAB function for evaluating matrix U.
f unc t i on U = U_matrix(theta)
U = [cos(theta) -s i n(theta) 0; s i n(theta) cos(theta) 0; 0 0 1];
R MATLAB function for evaluating matrix V .
f unc t i on V = V_matrix(al pha)
V = [1 0 0; 0 cos(al pha) -s i n(al pha); 0 s i n(al pha) cos(al pha)];
S MATLAB function for expressing a spatial vector in cross
form.
f unc t i on 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 ar ;
c l c ;
%% Figure print setup
c l e ar
10
% User-defined Parameters
print_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
% 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;
end
25
%% INITIALISATION AND PARAMETER DEFINITION:
% =========================================
% System parameter initialization
30 system_parameters_plant;
n = 100;
Q = zeros (6,1);
Q_d = zeros (6,n); % desired Q
Q_e = zeros (6,n); % evaluated Q
35
f or 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 nan(Q_all))))
pause;
50 end
end
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 or i=1:6
f i gur e (i);
c l f
pl ot (1:n,Q_d(i,:),-b,1:n,Q_e(i,:),-r);
65 gri d on
t i t l e ([str{i} vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
xl abe l (Time $\rightarrow$,Interpreter,latex,FontName,...
55
Times New Roman,FontSize,fontsize);
70 yl abe l ([str{i} $\rightarrow$],Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
l egend({str{i},strd{i}},...
Interpreter,latex,FontName,Times New Roman,FontSize,12,...
Color,none,Location,Best);
75 end
f or i=1:6
f i gur e (i+6);
c l f
80 pl ot (1:n,Q_d(i,:)-Q_e(i,:),-b);
gri d on
t i t l e ([str{i+6} vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
xl abe l (Time $\rightarrow$,Interpreter,latex,FontName,...
85 Times New Roman,FontSize,fontsize);
yl abe l ([str{i+6} $\rightarrow$],Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
end
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;
end
100 i f (print_flag)
f or j=1:12
f i gur e (j);
export_fig([print_dir prefix print_str{j}],-pdf,...
-transparent);
105 end
end
U MATLAB function for testing inverse and forward dynam-
ics.
% MATLAB Code for testing the forward and inverse dynamics algorithms
% of 6 DOF Stanford Arm.
% INPUTS : Initial and final pose of the robot end effector
% PARAMETERS : Robot Geometric and Inertial Parameters
5 % OUTPUTS : Required torque and resulting acceleration
% AUTHOR : Priyanshu Agarwal
56
%% INITIAL CLEANING
10 % ==============================
c l e ar ;
c l c ;
%% Figure print setup
15 c l e ar
% User-defined Parameters
print_flag = 0; % all plots will be printed as pdf if 1
20 % (only valid if animation_flag = 1)
i f print_flag
addpath(/home/priyanshu/Dropbox/MATLAB/export_fig);
% Directory to print the pdf files
25 print_dir = [/home/priyanshu/Dropbox/UT_Courses/2013_Spring/,...
Advanced_Dynamics_of_Robotic_Systems/Project/Project_Report/figures];
fontsize = 18;
e l s e
fontsize = 14;
30 end
%% INITIALISATION AND PARAMETER DEFINITION:
% =========================================
% System parameter initialization
35 system_parameters_plant;
%% Controller Variables
fc = 100; % controller frequency in Hz
dtc = 1/fc; % time step of controller
40
%% Simulator Variables
fs = 1000; % dynamics simulator frequency in Hz
%% Plan Path
45 p0_E_i = [0.5; 0; -0.1];
Q_i = zeros (6,1);
Q_dot_i = zeros (6,1);
Q_ddot_i = zeros (6,1);
50 p0_E_f = [-0.4; 0.1; 0.1];
Q_f = zeros (6,1);
Q_dot_f = zeros (6,1);
Q_ddot_f = zeros (6,1);
ti = 0;
55 tf = 10;
path_scale = 1;
57
Q_c = 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);
60
i = 1;
T = ti:dtc:tf;
% joint acceleration used for inverse dynamics
Q_ddot_ID = zeros (6,l e ngt h(T));
65 % joint acceleration evaluated using forward dynamics
Q_ddot_FD = zeros (6,l e ngt h(T));
%% Evaluating forward and inverse dynamics for the planned path
f or t=ti:dtc:tf
70 Q_id = Q_c
*
[1;t;t2;t3;t4;t5];
Q_dot_id = Q_c
*
[0;1;2
*
t;3
*
t2;4
*
t3;5
*
t4];
Q_ddot_id = Q_c
*
[0;0;2;6
*
t;12
*
t2;20
*
t3];
[Tau,s0_i_hat,I0_i_hat] = inverse_dynamics(Q_id,Q_dot_id,Q_ddot_id,...
P_DH,Mps);
75 Q_ddot_fd = forward_dynamics(Tau,Q_id,Q_dot_id,P_DH,Mps);
Q_ddot_ID(:,i) = Q_ddot_id;
Q_ddot_FD(:,i) = Q_ddot_fd;
i = i+1;
end
80
str = {$\ddot{\theta}_1$,$\ddot{\theta}_2$,$\ddot{r}_3$,...
$\ddot{\theta}_4$,$\ddot{\theta}_5$,$\ddot{\theta}_6$,...
$\ddot{\theta}_{1e}$,$\ddot{\theta}_{2e}$,$\ddot{r}_{3e}$,...
$\ddot{\theta}_{4e}$,$\ddot{\theta}_{5e}$,$\ddot{\theta}_{6e}$};
85
strd = {$\ddot{\theta}_{1d}$,$\ddot{\theta}_{2d}$,$\ddot{r}_{3d}$,...
$\ddot{\theta}_{4d}$,$\ddot{\theta}_{5d}$,$\ddot{\theta}_{6d}$};
f or i=1:6
90 f i gur e (i);
c l f
pl ot (T,Q_ddot_ID(i,:),-b,T,Q_ddot_FD(i,:),-r);
gri d on
t i t l e ([str{i} vs Time],Interpreter,latex,...
95 FontName,Times New Roman,FontSize,fontsize);
xl abe l (Time $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
yl abe l ([str{i} $\rightarrow$],Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
100 l egend({str{i},strd{i}},...
Interpreter,latex,FontName,Times New Roman,FontSize,12,...
Color,none,Location,Best);
end
105 f or i=1:6
f i gur e (i+6);
c l f
pl ot (T,Q_ddot_ID(i,:)-Q_ddot_FD(i,:),-b);
58
gri d on
110 t i t l e ([str{i+6} vs Time],Interpreter,latex,...
FontName,Times New Roman,FontSize,fontsize);
xl abe l (Time $\rightarrow$,Interpreter,latex,FontName,...
Times New Roman,FontSize,fontsize);
yl abe l ([str{i+6} $\rightarrow$],Interpreter,latex,FontName,...
115 Times New Roman,FontSize,fontsize);
end
%% Printing the results in pdf files
120 print_str={theta_ddot_1,theta_ddot_2,r_ddot_3,theta_ddot_4,...
theta_ddot_5,theta_ddot_6,...
theta_ddot_1_e,theta_ddot_2_e,r_ddot_3_e,theta_ddot_4_e,...
theta_ddot_5_e,theta_ddot_6_e};
125 i f print_flag
prefix = /test_dynamics;
end
i f (print_flag)
130 f or j=1:12
f i gur e (j);
export_fig([print_dir prefix print_str{j}],-pdf,...
-transparent);
end
135 end
59