Examples

Here are some examples for Forward and Inverse Dynamics Trajectories, and Control Simulation.

Contents

Inverse Dynamics Trajectory

using ModernRoboticsBook

import LinearAlgebra
const linalg = LinearAlgebra;

Create a trajectory to follow using functions from Chapter 9:

thetastart = [0, 0, 0]

thetaend = [π / 2, π / 2, π / 2]

Tf = 3

N = 1000

method = 5

traj = JointTrajectory(thetastart, thetaend, Tf, N, method)

thetamat = copy(traj)
dthetamat = zeros(1000, 3)
ddthetamat = zeros(1000, 3)

dt = Tf / (N - 1.0)

for i = 1:size(traj, 1) - 1
    dthetamat[i + 1, :] = (thetamat[i + 1, :] - thetamat[i, :]) / dt
    ddthetamat[i + 1, :] = (dthetamat[i + 1, :] - dthetamat[i, :]) / dt
end

Initialize robot description (example with 3 links):

g = [0, 0, -9.8]

Ftipmat = ones(N, 6)

M01 = [ 1 0 0        0 ;
        0 1 0        0 ;
        0 0 1 0.089159 ;
        0 0 0        1 ]

M12 = [ 0 0 1    0.28 ;
        0 1 0 0.13585 ;
       -1 0 0       0 ;
        0 0 0       1 ]

M23 = [ 1 0 0       0 ;
        0 1 0 -0.1197 ;
        0 0 1   0.395 ;
        0 0 0       1 ]

M34 = [ 1 0 0       0 ;
        0 1 0       0 ;
        0 0 1 0.14225 ;
        0 0 0       1 ]

Mlist = [M01, M12, M23, M34]

G1 = linalg.Diagonal([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7])
G2 = linalg.Diagonal([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393])
G3 = linalg.Diagonal([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275])

Glist = [G1, G2, G3]

Slist = [ 1 0 1      0 1     0 ;
          0 1 0 -0.089 0     0 ;
          0 1 0 -0.089 0 0.425 ]'

taumat = InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, Ftipmat, Mlist, Glist, Slist)

Plot the joint forces/torques:

using Plots
gr()

timestamp = range(1, Tf, length=N)

plot(timestamp, taumat[:, 1], linewidth=2, label="Tau 1")
plot!(timestamp, taumat[:, 2], linewidth=2, label="Tau 2")
plot!(timestamp, taumat[:, 3], linewidth=2, label="Tau 3")

xlabel!("Time")
ylabel!("Torque")
title!("Plot of Torque Trajectories")

inverse_dynamics_trajectory

Forward Dynamics Trajectory

dt = 0.1

intRes = 8

thetalist = [0.1, 0.1, 0.1]

dthetalist = [0.1, 0.2, 0.3]

taumat = [[3.63, -6.58, -5.57], [3.74, -5.55,  -5.5],
          [4.31, -0.68, -5.19], [5.18,  5.63, -4.31],
          [5.85,  8.17, -2.59], [5.78,  2.79,  -1.7],
          [4.99,  -5.3, -1.19], [4.08, -9.41,  0.07],
          [3.56, -10.1,  0.97], [3.49, -9.41,  1.23]]

taumat = cat(taumat..., dims=2)'

thetamat, dthetamat = ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g,
                                                Ftipmat, Mlist, Glist, Slist, dt, intRes)

Plot the joint angle/velocities:

theta1 = thetamat[:, 1]
theta2 = thetamat[:, 2]
theta3 = thetamat[:, 3]

dtheta1 = dthetamat[:, 1]
dtheta2 = dthetamat[:, 2]
dtheta3 = dthetamat[:, 3]

N = size(taumat, 1)
Tf = size(taumat, 1) * dt

timestamp = range(0, Tf, length=N)

plot(timestamp, theta1, linewidth=2, label="Theta1")
plot!(timestamp, theta2, linewidth=2, label="Theta2")
plot!(timestamp, theta3, linewidth=2, label="Theta3")

plot!(timestamp, dtheta1, linewidth=2, label="DTheta1")
plot!(timestamp, dtheta2, linewidth=2, label="DTheta2")
plot!(timestamp, dtheta3, linewidth=2, label="DTheta3")

xlabel!("Time")
ylabel!("Joint Angles/Velocities")
title!("Plot of Joint Angles and Joint Velocities")

forward_dynamics_trajectory

Simulate Control

Create a trajectory to follow:

thetaend = [π / 2, π, 1.5 * π]

Tf = 1
dt = 0.01
N = round(Int, Tf / dt)
method = 5

traj = JointTrajectory(thetalist, thetaend, Tf, N, method)

thetamatd = copy(traj)
dthetamatd = zeros(N, 3)
ddthetamatd = zeros(N, 3)

dt = Tf / (N - 1)

for i = 1:size(traj, 1)-1
    dthetamatd[i + 1, :] = (thetamatd[i + 1, :] - thetamatd[i, :]) / dt
    ddthetamatd[i + 1, :] = (dthetamatd[i + 1, :] - dthetamatd[i, :]) / dt
end

Create a (possibly) wrong robot description:

gtilde = [0.8, 0.2, -8.8]

Mhat01 = [1 0 0   0 ;
          0 1 0   0 ;
          0 0 1 0.1 ;
          0 0 0   1 ]

Mhat12 = [ 0 0 1 0.3 ;
           0 1 0 0.2 ;
          -1 0 0   0 ;
           0 0 0   1 ]

Mhat23 = [1 0 0    0 ;
          0 1 0 -0.2 ;
          0 0 1  0.4 ;
          0 0 0    1 ]

Mhat34 = [1 0 0   0 ;
          0 1 0   0 ;
          0 0 1 0.2 ;
          0 0 0   1 ]

Mtildelist = [Mhat01, Mhat12, Mhat23, Mhat34]

Ghat1 = linalg.Diagonal([0.1, 0.1, 0.1, 4, 4, 4])
Ghat2 = linalg.Diagonal([0.3, 0.3, 0.1, 9, 9, 9])
Ghat3 = linalg.Diagonal([0.1, 0.1, 0.1, 3, 3, 3])

Gtildelist = [Ghat1, Ghat2, Ghat3]

Ftipmat = ones(size(traj, 1), 6)

Kp = 20
Ki = 10
Kd = 18

intRes = 8

taumat, thetamat = SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist,
                                   Slist, thetamatd, dthetamatd, ddthetamatd, gtilde,
                                   Mtildelist, Gtildelist, Kp, Ki, Kd, dt, intRes)

Finally, plot the results:

N, links = size(thetamat)

Tf = N * dt

timestamp = range(0, Tf, length=N)

plot()

for i = 1:links
    plot!(timestamp, thetamat[:, i], lw=2, linestyle=:dash, label="ActualTheta $i")
    plot!(timestamp, thetamatd[:, i], lw=2, linestyle=:dot, label="DesiredTheta $i")
end

xlabel!("Time")
ylabel!("Joint Angles")
title!("Plot of Actual and Desired Joint Angles")

simulated_control