Project Report Robot Dynamics

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


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


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


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


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


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


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/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


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


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


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


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


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))


pa us e;

en d

i f( abs( s i n (theta_5A)* s i n (theta_6A)+P(3,2))>10(-10))


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


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


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


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


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


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


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


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


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


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


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


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


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


26/60

0 1 2 3 4 5 6 7 8 9 108

6

4

2

0

2

4

6x 10


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


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


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


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


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


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


28/60

0 1 2 3 4 5 6 7 8 9 102

1

0

1

2

3

4

5

6

7x 10


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


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


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


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


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


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


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



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


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


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


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


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,...


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


35/60

g r i d on

235 t i t l e ([str{i+6} vs Time],Interpreter,latex,...




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}},...


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,...




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,...



270 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,...



34


36/60


y l a b e l ([Energy (Joules) $\rightarrow$],Interpreter,latex,...


l e g e n d ({Total Energy Diff,Instantaneous Actuator Energy Input},...


290 FontSize,12,...


% 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);



y l a b e l ([Energy (Joules) $\rightarrow$],Interpreter,latex,...


305 l e g e n d ({Total Energy,Actuator Input Energy},...


FontSize,12,...


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,...




320 y l a b e l ([Energy Error (Joules) $\rightarrow$],Interpreter,...

latex,...


% 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,...




35


37/60

y l a b e l ([FeedForward Torque (Nm) $\rightarrow$],Interpreter,...

latex,...


l e g e n d ({$\tau_1$,$\tau_2$,$f_3$,$\tau_4$,$\tau_5$,...

340 $\tau_6$},...


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,...


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,...


l e g e n d ({$\tau_1$,$\tau_2$,$f_3$,$\tau_4$,$\tau_5$,...

360 $\tau_6$},...


FontSize,12,...


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,...




375 y l a b e l ([$p0_e$ (m) $\rightarrow$],Interpreter,latex,...

FontName,...


l e g e n d ({$X$,$Y$,$Z$},...


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


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,...


y l a b e l ([$p0_e$ error (m) $\rightarrow$],Interpreter,latex,...

FontName,...


400 l e g e n d ({$X$,$Y$,$Z$},...


FontSize,12,...


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


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


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


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


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


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


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+4) = theta_5B;


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


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))


pa us e;

en d

i f( abs( s i n (theta_5A)* c o s (theta_6A)-P(3,1))>10(-10))


pa us e;

en d

i f( abs( s i n (theta_5A)* s i n (theta_6A)+P(3,2))>10(-10))


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



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


Alpha = P_DH(:,3); % robot alpha_i vector

R(3) = r3;

43


45/60


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


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



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


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



%% 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 f(max(max( abs(I0c_i_hat(:,:,i)-...



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))


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


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(3) = r3;

10 Alpha = P_DH(:,3); % robot alpha_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)*...


35 A0_i(:,:,6) = A0_i(:,:,5)*...


%% 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


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,...


75 z l a b e l (Z0 $\rightarrow$,Interpreter,latex,FontName,...


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


%% 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


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;


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


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



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


105 i f(max(max( abs(I0c_i_hat(:,:,i)-...



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


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


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


53


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


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,...



55


57/60


70 y l a b e l ([str{i} $\rightarrow$],Interpreter,latex,FontName,...


l e g e n d ({str{i},strd{i}},...

Interpreter,latex,FontName,Times New Roman,FontSize,12,...


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,...




y l a b e l ([str{i+6} $\rightarrow$],Interpreter,latex,FontName,...


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

Project Report Robot Dynamics

Documents

Transcript of Project Report Robot Dynamics