# We Helped With This MATLAB Programming Assignment: Have A Similar One?

SOLVED
Category Programming MATLAB Undergraduate Solved Matlab Homework Help In Usa

## Short Assignment Requirements

can you do Exercise 1and exercise 2 in matlab and make the script as pdf

## Assignment Image

Chapter 3 Rigid Body Motions R = (3x3) rotation matrix, SO(3) special orthogonal group 3. omg = (3x1) angular velocity expc3 = (3x1) vector of exponential coordinates (what) so3mat = (3x3) skew symmetric matrix representation of wo or (what), set of skew symmetric called so(3) T = (4x4) homogeneous transformation matrix SE(3), special Euclidean group (3) - which is a (4x4) [AdT] = (6x6) adjoint representation of T V, S = (6x1) twist and screw se3mat = (4x4) matrix representation of [V] or [S]0, se(3) invR = RotInv(R) - computes the inverse of (3x3) rotation matrix using the transpose. so3mat= VecToso3(omg) - returns the (3x3) skew symmetric matrix corresponding to the angular rotation omega. omg = so3To Vec(so3mat) - returns (3x1) vector corresponding to (3x3) skew symmetric matrix so3mat. [omghat, theta] = AxisAng3(expc3) - extracts the rotation axis what and the rotation from the 3 vector (@har) of exponential coordinates expc3. R = MatrixExp3 (so3mat) - computes the (3x3) rotation matrix R corresponding to the matrix exponential expressed in (3x3) skew symmetric form. so3mat = MatrixLog3(R) - computes the (3x3) skew symmetric matrix logarithm (@hat0) from the (3x3) rotation matrix, SO(3). T= RpToTrans(R, P) - builds the (4x4) homogeneous transformation matrix from (3x3) rotation matrix and (3x1) position vector. [R, p] = TransToRp(T) - extracts the rotation matrix and positon vector from the transformation matrix. InvT = TransInv(T) - computes the inverse of a transformation matrix. se3mat = VecTose3(V) - returns the (4x4) se(3) [V] [[w] v; 0 0] matrix corresponding to a 6 vector twist V=se3To Vec(se3mat) returns the (6x1) vector corresponding to the (4x4) se(3) matrix AdT = Adjoint(T) - computes (6x6) adjoint of homogenous transformation matrix, [AdT] S = ScrewToAxis (q,s,h) - returns a normalized screw axis S of a screw described by a unit vector s in the direction of the screw axis, located at the point q from the origin of the frame, with a pitch h [S, theta] = Axis Ang6(expc6) - returns the normalized screw axis and the distance travelled along the axis from an input of a (6x1) vector of exponential coordinates (e.g., MatrixExp60) produces (4x4) se(3) rep of [S0], and se3 ToVec() produces 6 vector). T = MatrixExp6(se3mat) - computes the homogenous transformation matrix corresponding to the matrix exponential. se3mat = MatrixLog6(T) computes the (4x4) matrix logarithm [S]0 se3 mat of the homogenous transformation matrix.

## Assignment Description

MEM 455 Lab 4 – Forward Kinematics using Corke’s Robotics Toolbox

Please read carefully. The punctuation ( . *  ‘ ) is often needed for the code to run correctly.

Load the ETS2 package. ETS2 allows for the simulation of 2d spatial transformations.

>> import ETS2. *

-          Set link length to 1 and define ‘E’ as a link on a revolute joint in x-y plane. ‘E’ is your link or robot name.

>> a1 = 1

>> E = Rz(‘q1’)*Tx(a1)

The argument in Rz (rotation around z) is a string and indicates ‘q2’ as a joint variable that can be adjusted. The argument in Tx (translation in x) is the link’s length which is set as the constant a1.

The configuration (forward kinematics) for a particular angle q1 can be determined using the method ***.fkine  (yes, there is a period in front of fkine). The option ‘deg’ sets the variable from radians to degrees. ‘xxx’ is your robot’s name.

-          Determine configuration for a joint angle of 30 degrees.

>> E.fkine(30, ‘deg’)

-          Plot the configuration

>> E.plot(30, ‘deg’)

-          Modify the figure so that the variable can be changed using a slider. This enables the user to learn how to position the joints to achieve desired configurations.

>>E.teach

A multi-link robot arm can be created by creating a series of R’s and T’s

>> a1 = 1; a2 = 2;

>> E2 = Rz(‘q1’)*Tx(a1)*Rz(‘q2’)*Ty(a2)

In this case, the second link is aligned with the y axis.

The joint angles are now specified by a [1 x r ] vector, where r is the number of joint variables.

>>E2.fkine([30 45], ‘deg’)

>>E2.plot([30 45], ‘deg’)

>>E2.teach

The type of linkage can be determined from its structure

>>E2.structure

Prismatic joints can be defined by using a variable as an argument in the Tz or Ty property.

>> E3 = Rz(‘q1’)*Tx(a1)*Ty(‘q2’)

To reduce confusion in units, it is recommended to use radians for angles.

>> E3.fkine([pi/2 2])

Plotting a prismatic joint seems to cause problems. The solution is supposed to be to define the plot’s dimensions using the argument ‘workspace’, [6 x 1], but I haven’t had consistent luck.

Exercise 1.  Simulate a 3R robot with link lengths of 1, 2, and 3. Plot the kinematics for joint angles of pi/3, pi/4, pi/5. Submit using a PDF in LiveScript. (5 pts)

Clear the ETS2 package and import the 3d package, ETS3

>> clear import

>> clear figure

>> import ETS3.*

Define link lengths for a 6R robot

>> L1 = 0; L2 = -0.2337; L3 = 0.4318; L4 = 0.0203; L5=0.0837; L6 = 0.4318;

I believe Tz with a length of 0 is needed to put the revolute joint at the base.

Define robot

>> E3d = Tz(L1)*Rz(‘q1’)*Ry(‘q2’)*Ty(L2)*Tz(L3)*Ry(‘q3’)*Tx(L4)*Ty(L5)*Tz(L6)*Ry(‘q5’)*Rz(‘q6’);

Consider the forward kinematics and teaching mode

>> E3d.fkine([0 0 0 0 0 0])  ‘ puts robot in zero configuration

>> E3d.plot([0 0 0 0 0 0])

>> E3d.teach

This approach is intuitive, but cumbersome.

Exercise 2.  (5 pts)

2a) Using this sequential assignment, create a model of the linkage of your 3R robot.   Use accurate link lengths. For your final project, you should do the same analysis on your modified robot.

2b) Determine the configuration matrix and plot the zero configuration.

2c) Use the teach mode to determine the joint angles required to attain at least 3 configurations that are both important for your task, and near the limits of the task space.

## Assignment Code

``````
%*** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***

% Takes V: 6-vector spatial velocity.
% Returns adV: The corresponding 6x6 matrix.
% Used to calculate the Lie bracket [V1, V2] = [adV1]V2
% Example Input:
%{
clear;clc;
V = [1; 2; 3; 4; 5; 6];
%}
% Output:
%     0    -3     2     0     0     0
%     3     0    -1     0     0     0
%    -2     1     0     0     0     0
%     0    -6     5     0    -3     2
%     6     0    -4     3     0    -1
%    -5     4     0    -2     1     0

omgmat = VecToso3(V(1:3));
adV = [omgmat, zeros(3); VecToso3(V(4:6)), omgmat];
end
``````

## Assignment Code

``````
%*** CHAPTER 3: RIGID-BODY MOTIONS ***

% Takes T a transformation matrix SE3.
% Example Input:
%{
clear;clc;
T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]];
%}
% Output:
%     1     0     0     0     0     0
%     0     0    -1     0     0     0
%     0     1     0     0     0     0
%     0     0     3     1     0     0
%     3     0     0     0     0    -1
%     0     0     0     0     1     0

[R, p] = TransToRp(T);
AdT = [R, zeros(3); VecToso3(p) * R, R];
end
``````

## Assignment Code

``````
%*** CHAPTER 3: RIGID-BODY MOTIONS ***

function [omghat, theta] = AxisAng3(expc3)
% Takes A 3-vector of exponential coordinates for rotation.
% Returns the unit rotation axis omghat and the corresponding rotation
% angle theta.
% Example Input:
%{
clear;clc;
expc3 = [1; 2; 3];
[omghat, theta] = AxisAng3(expc3)
%}
% Output:
% omghat =
%    0.2673
%    0.5345
%    0.8018
% theta =
%    3.7417

theta = norm(expc3);
omghat = expc3 / theta;
end
``````

## Assignment Code

``````
%*** CHAPTER 3: RIGID-BODY MOTIONS ***

function [S,theta] = AxisAng6(expc6)
% Takes a 6-vector of exponential coordinates for rigid-body motion
% S*theta.
% Returns S: the corresponding normalized screw axis,
%         theta: the distance traveled along/about S.
% Example Input:
%{
clear;clc;
expc6 = [1; 0; 0; 1; 2; 3];
[S, theta] = AxisAng6(expc6)
%}
% Output:
% S =
%     1
%     0
%     0
%     1
%     2
%     3
% theta =
%     1

theta = norm(expc6(1:3));
if NearZero(theta)
theta = norm(expc6(4:6));
end
S = expc6 / theta;
end
``````

## Assignment Code

``````
%*** CHAPTER 9: TRAJECTORY GENERATION ***

function traj = CartesianTrajectory(Xstart,Xend,Tf,N,method)
% Takes Xstart: The initial end-effector configuration,
%       Xend: The final end-effector configuration,
%       Tf: Total time of the motion in seconds from rest to rest,
%       N: The number of points N > 1 (Start and stop) in the discrete
%          representation of the trajectory,
%       method: The time-scaling method, where 3 indicates cubic
%               (third-order polynomial) time scaling and 5 indicates
%               quintic (fifth-order polynomial) time scaling.
% Returns traj: The discretized trajectory as a list of N matrices in SE(3)
%               separated in time by Tf/(N-1). The first in the list is
%               Xstart and the Nth is Xend .
% This function is similar to ScrewTrajectory, except the origin of the
% end-effector frame follows a straight line, decoupled from the rotational
% motion.
% Example Input:
%{
clear;clc;
Xstart = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 1]; [0, 0, 0, 1]];
Xend = [[0, 0, 1, 0.1]; [1, 0, 0, 0]; [0, 1, 0, 4.1]; [0, 0, 0, 1]];
Tf = 5;
N = 4;
method = 5;
traj = CartesianTrajectory(Xstart,Xend,Tf,N,method)
%}
% Output:
% traj =
%    1.0000         0         0    1.0000
%         0    1.0000         0         0
%         0         0    1.0000    1.0000
%         0         0         0    1.0000
%
%    0.9366   -0.2140    0.2774    0.8111
%    0.2774    0.9366   -0.2140         0
%   -0.2140    0.2774    0.9366    1.6506
%         0         0         0    1.0000
%
%    0.2774   -0.2140    0.9366    0.2889
%    0.9366    0.2774   -0.2140         0
%   -0.2140    0.9366    0.2774    3.4494
%         0         0         0    1.0000
%
%   -0.0000    0.0000    1.0000    0.1000
%    1.0000   -0.0000    0.0000         0
%    0.0000    1.0000   -0.0000    4.1000
%         0         0         0    1.0000

timegap = Tf / (N - 1);
traj = cell(1,N);
[Rstart, pstart] = TransToRp(Xstart);
[Rend, pend] = TransToRp(Xend);
for i = 1:N
if method == 3
s = CubicTimeScaling(Tf,timegap * (i - 1));
else
s = QuinticTimeScaling(Tf,timegap * (i - 1));
end
traj{i} ...
= [Rstart * MatrixExp3(MatrixLog3(Rstart' * Rend) * s), ...
pstart + s * (pend - pstart); 0, 0, 0, 1];
end
end
``````

## Assignment Code

``````
%*** CHAPTER 11: ROBOT CONTROL ***

function taulist = ComputedTorque(thetalist,dthetalist,eint,g,Mlist, ...
Glist,Slist,thetalistd,dthetalistd, ...
ddthetalistd,Kp,Ki,Kd)
% Takes thetalist: n-vector of joint variables,
%       dthetalist: n-vector of joint rates,
%       eint: n-vector of the time-integral of joint errors,
%       g: Gravity vector g,
%       Mlist: List of link frames {i} relative to {i-1} at the home
%              position,
%       Glist: Spatial inertia matrices Gi of the links,
%       Slist: Screw axes Si of the joints in a space frame,
%       thetalistd: n-vector of reference joint variables,
%       dthetalistd: n-vector of reference joint velocities,
%       ddthetalistd: n-vector of reference joint accelerations,
%       Kp: The feedback proportional gain (identical for each joint),
%       Ki: The feedback integral gain (identical for each joint),
%       Kd: The feedback derivative gain (identical for each joint).
% Returns taulist: The vector of joint forces/torques computed by the
%                  feedback linearizing controller at the current instant.
% Example Input:
%{
clc;clear;
thetalist = [0.1; 0.1; 0.1];
dthetalist = [0.1; 0.2; 0.3];
eint = [0.2; 0.2; 0.2];
g = [0; 0; -9.8];
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]];
G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
Glist = cat(3,G1,G2,G3);
Mlist = cat(4,M01,M12,M23,M34);
Slist = [[1; 0; 1;      0; 1;     0], ...
[0; 1; 0; -0.089; 0;     0], ...
[0; 1; 0; -0.089; 0; 0.425]];
thetalistd = [1; 1; 1];
dthetalistd = [2; 1.2; 2];
ddthetalistd = [0.1; 0.1; 0.1];
Kp = 1.3;
Ki = 1.2;
Kd = 1.1;
taulist ...
= ComputedTorque(thetalist,dthetalist,eint,g,Mlist,Glist,Slist, ...
thetalistd,dthetalistd,ddthetalistd,Kp,Ki,Kd)
%}
% Output:
% taulist =
%  133.0053
%  -29.9422
%   -3.0328

e = thetalistd - thetalist;
taulist ...
= MassMatrix(thetalist,Mlist,Glist,Slist) ...
* (Kp * e + Ki * (eint + e) + Kd * (dthetalistd - dthetalist)) ...
+ InverseDynamics(thetalist,dthetalist,ddthetalistd,g,zeros(6,1), ...
Mlist,Glist,Slist);
end
``````

## Assignment Code

``````
%*** CHAPTER 9: TRAJECTORY GENERATION ***

function s = CubicTimeScaling(Tf,t)
% Takes Tf: Total time of the motion in seconds from rest to rest,
%       t: The current time t satisfying 0 < t < Tf.
% Returns s: The path parameter s(t) corresponding to a third-order
%            polynomial motion that begins and ends at zero velocity.
% Example Input:
%{
clear;clc;
Tf = 2;
t = 0.6;
s = CubicTimeScaling(Tf,t)
%}
% Output:
% s =
%    0.2160

s = 3 * (t / Tf) ^ 2 - 2 * (t / Tf) ^ 3;
end
``````

## Assignment Code

``````
%*** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***

function JTFtip = EndEffectorForces(thetalist,Ftip,Mlist,Glist,Slist)
% Takes thetalist: A list of joint variables,
%       Ftip: Spatial force applied by the end-effector expressed in frame
%             {n+1},
%       Mlist: List of link frames i relative to i-1 at the home position,
%       Glist: Spatial inertia matrices Gi of the links,
%       Slist: Screw axes Si of the joints in a space frame.
% Returns JTFtip: The joint forces and torques required only to create the
%                 end-effector force Ftip.
% This function calls InverseDynamics with g = 0, dthetalist = 0, and
% ddthetalist = 0.
% Example Input (3 Link Robot):
%{
clear;clc;
thetalist = [0.1; 0.1; 0.1];
Ftip = [1; 1; 1; 1; 1; 1];
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]];
G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
Glist = cat(3,G1,G2,G3);
Mlist = cat(4,M01,M12,M23,M34);
Slist = [[1; 0; 1;      0; 1;     0], ...
[0; 1; 0; -0.089; 0;     0], ...
[0; 1; 0; -0.089; 0; 0.425]];
JTFtip = EndEffectorForces(thetalist,Ftip,Mlist,Glist,Slist)
%}
% Output:
% JTFtip =
%    1.4095
%    1.8577
%    1.3924

n = size(thetalist,1);
JTFtip = InverseDynamics(thetalist,zeros(n,1),zeros(n,1),[0; 0; 0], ...
Ftip,Mlist,Glist,Slist);
end
``````

## Assignment Code

``````
%*** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***

function [thetalistNext, dthetalistNext] ...
= EulerStep(thetalist,dthetalist,ddthetalist,dt)
% Takes thetalist: n-vector of joint variables,
%       dthetalist: n-vector of joint rates,
%       ddthetalist: n-vector of joint accelerations,
%       dt: The timestep delta t.
% Returns thetalistNext: Vector of joint variables after dt from first
%                        order Euler integration,
%         dthetalistNext: Vector of joint rates after dt from first order
%                         Euler integration.
% Example Inputs (3 Link Robot):
%{
clear;clc;
thetalist = [0.1; 0.1; 0.1];
dthetalist = [0.1; 0.2; 0.3];
ddthetalist = [2; 1.5; 1];
dt = 0.1;
[thetalistNext, dthetalistNext] = EulerStep(thetalist,dthetalist, ...
ddthetalist,dt)
%}
% Output:
% thetalistNext =
%    0.1100
%    0.1200
%    0.1300
% dthetalistNext =
%    0.3000
%    0.3500
%    0.4000

thetalistNext = thetalist + dt * dthetalist;
dthetalistNext = dthetalist + dt * ddthetalist;
end
``````

## Assignment Code

``````
%*** CHAPTER 4: FORWARD KINEMATICS ***

function T = FKinBody(M,Blist,thetalist)
% Takes M: the home configuration (position and orientation) of the
%          end-effector,
%       Blist: The joint screw axes in the end-effector frame when the
%              manipulator is at the home position,
%       thetalist: A list of joint coordinates.
% Returns T in SE(3) representing the end-effector frame when the joints
% are at the specified coordinates (i.t.o Body Frame).
% Example Inputs:
%{
clear;clc;
M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]];
Blist = [[0; 0; -1; 2; 0; 0], [0; 0; 0; 0; 1; 0], [0; 0; 1; 0; 0; 0.1]];
thetalist = [pi / 2; 3; pi];
T = FKinBody(M,Blist,thetalist)
%}
% Output:
% T =
%   -0.0000    1.0000         0   -5.0000
%    1.0000    0.0000         0    4.0000
%         0         0   -1.0000    1.6858
%         0         0         0    1.0000

T = M;
for i = 1:size(thetalist)
T = T * MatrixExp6(VecTose3(Blist(:,i) * thetalist(i)));
end
end
``````

## Assignment Code

``````
%*** CHAPTER 4: FORWARD KINEMATICS ***

function T = FKinSpace(M,Slist,thetalist)
% Takes M: the home configuration (position and orientation) of the
%          end-effector,
%       Slist: The joint screw axes in the space frame when the manipulator
%              is at the home position,
%       thetalist: A list of joint coordinates.
% Returns T in SE(3) representing the end-effector frame, when the joints
% are at the specified coordinates (i.t.o Space Frame).
% Example Inputs:
%{
clear;clc;
M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]];
Slist = [[0; 0;  1;  4; 0;    0], ...
[0; 0;  0;  0; 1;    0], ...
[0; 0; -1; -6; 0; -0.1]];
thetalist =[pi / 2; 3; pi];
T = FKinSpace(M,Slist,thetalist)
%}
% Output:
% T =
%   -0.0000    1.0000         0   -5.0000
%    1.0000    0.0000         0    4.0000
%         0         0   -1.0000    1.6858
%         0         0         0    1.0000

T = M;
for i = size(thetalist):-1:1
T = MatrixExp6(VecTose3(Slist(:,i) * thetalist(i))) * T;
end
end
``````

## Assignment Code

``````
%*** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***

function ddthetalist = ForwardDynamics(thetalist,dthetalist,taulist,g, ...
Ftip,Mlist,Glist,Slist)
% Takes thetalist: A list of joint variables,
%       dthetalist: A list of joint rates,
%       taulist: An n-vector of joint forces/torques,
%       g: Gravity vector g,
%       Ftip: Spatial force applied by the end-effector expressed in frame
%             {n+1},
%       Mlist: List of link frames i relative to i-1 at the home position,
%       Glist: Spatial inertia matrices Gi of the links,
%       Slist: Screw axes Si of the joints in a space frame.
% Returns ddthetalist: The resulting joint accelerations.
% This function computes ddthetalist by solving:
% Mlist(thetalist) * ddthetalist = taulist - c(thetalist,dthetalist) ...
%                                  - g(thetalist) - Jtr(thetalist) * Ftip
% Example Input (3 Link Robot):
%{
clear;clc;
thetalist = [0.1; 0.1; 0.1];
dthetalist = [0.1; 0.2; 0.3];
taulist = [0.5; 0.6; 0.7];
g = [0; 0; -9.8];
Ftip = [1; 1; 1; 1; 1; 1];
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]];
G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
Glist = cat(3,G1,G2,G3);
Mlist = cat(4,M01,M12,M23,M34);
Slist = [[1; 0; 1;      0; 1;     0], ...
[0; 1; 0; -0.089; 0;     0], ...
[0; 1; 0; -0.089; 0; 0.425]];
ddthetalist = ForwardDynamics(thetalist,dthetalist,taulist,g,Ftip, ...
Mlist,Glist,Slist)
%}
% Output:
% ddthetalist =
%   -0.9739
%   25.5847
%  -32.9150

ddthetalist = MassMatrix(thetalist,Mlist,Glist,Slist) ...
Mlist,Glist,Slist) ...
- GravityForces(thetalist,g,Mlist,Glist,Slist) ...
- EndEffectorForces(thetalist,Ftip,Mlist,Glist,Slist));
end
``````

## Assignment Code

``````
%*** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***

function [thetamat, dthetamat] ...
= ForwardDynamicsTrajectory(thetalist,dthetalist,taumat,g, ...
Ftipmat,Mlist,Glist,Slist,dt,intRes)
% Takes thetalist: n-vector of initial joint variables,
%       dthetalist: n-vector of initial joint rates,
%       taumat: An N x n matrix of joint forces/torques, where each row is
%               the joint effort at any time step,
%       g: Gravity vector g,
%       Ftipmat: An N x 6 matrix of spatial forces applied by the
%                end-effector (If there are no tip forces, the user should
%                input a zero and a zero matrix will be used),
%       Mlist: List of link frames {i} relative to {i-1} at the home
%              position,
%       Glist: Spatial inertia matrices Gi of the links,
%       Slist: Screw axes Si of the joints in a space frame,
%       dt: The timestep between consecutive joint forces/torques,
%       intRes: Integration resolution is the number of times integration
%               (Euler) takes places between each time step. Must be an
%               integer value greater than or equal to 1.
% Returns thetamat: The N x n matrix of robot joint angles resulting from
%                   the specified joint forces/torques,
%         dthetamat: The N x n matrix of robot joint velocities.
% This function simulates the motion of a serial chain given an open-loop
% history of joint forces/torques. It calls a numerical integration
% procedure that uses ForwardDynamics.
% Example Inputs (3 Link Robot):
%{
clc;clear;
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]];
%Initialise robot description (Example with 3 links)
g = [0;0;-9.8];
Ftipmat = ones(size(taumat,1),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]];
G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
Glist = cat(3,G1,G2,G3);
Mlist = cat(4,M01,M12,M23,M34);
Slist = [[1; 0; 1;      0; 1;     0], ...
[0; 1; 0; -0.089; 0;     0], ...
[0; 1; 0; -0.089; 0; 0.425]];
dt = 0.1;
intRes = 8;
[thetamat,dthetamat] ...
= ForwardDynamicsTrajectory(thetalist,dthetalist,taumat,g,Ftipmat, ...
Mlist,Glist,Slist,dt,intRes);
%Output using matplotlib to plot the joint forces/torques
Tf = size(taumat,1);
time=0:(Tf/size(thetamat,1)):(Tf-(Tf/size(thetamat,1)));
plot(time,thetamat(:,1),'b')
hold on
plot(time,thetamat(:,2),'g')
plot(time,thetamat(:,3),'r')
plot(time,dthetamat(:,1),'c')
plot(time,dthetamat(:,2),'m')
plot(time,dthetamat(:,3),'y')
title('Plot of Joint Angles and Joint Velocities')
xlabel('Time')
ylabel('Joint Angles/Velocities')
legend('Theta1','Theta2','Theta3','DTheta1','DTheta2','DTheta3')
%}

taumat = taumat';
Ftipmat = Ftipmat';
thetamat = taumat;
thetamat(:,1) = thetalist;
dthetamat = taumat;
dthetamat(:,1) = dthetalist;
for i = 1:size(taumat,2) - 1
for j = 1:intRes
ddthetalist = ForwardDynamics(thetalist,dthetalist,taumat(:,i), ...
g,Ftipmat(:,i),Mlist,Glist,Slist);
[thetalist, dthetalist] = EulerStep(thetalist,dthetalist, ...
ddthetalist,dt / intRes);
end
thetamat(:,i + 1) = thetalist;
dthetamat(:,i + 1) = dthetalist;
end
thetamat = thetamat';
dthetamat = dthetamat';
end
``````

## Assignment Code

``````
%*** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***

function grav = GravityForces(thetalist,g,Mlist,Glist,Slist)
% Takes thetalist: A list of joint variables,
%       g: 3-vector for gravitational acceleration,
%       Mlist: List of link frames i relative to i-1 at the home position,
%       Glist: Spatial inertia matrices Gi of the links,
%       Slist: Screw axes Si of the joints in a space frame.
% Returns grav: The joint forces/torques required to overcome gravity at
%               thetalist
% This function calls InverseDynamics with Ftip = 0, dthetalist = 0, and
% ddthetalist = 0.
% Example Input (3 Link Robot):
%{
clear;clc;
thetalist = [0.1; 0.1; 0.1];
g = [0; 0; -9.8];
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]];
G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
Glist = cat(3,G1,G2,G3);
Mlist = cat(4,M01,M12,M23,M34);
Slist = [[1; 0; 1;      0; 1;     0], ...
[0; 1; 0; -0.089; 0;     0], ...
[0; 1; 0; -0.089; 0; 0.425]];
grav = GravityForces(thetalist,g,Mlist,Glist,Slist)
%}
% Output:
% grav =
%   28.4033
%  -37.6409
%   -5.4416

n = size(thetalist,1);
grav = InverseDynamics(thetalist,zeros(n,1),zeros(n,1),g, ...
[0; 0; 0; 0; 0; 0],Mlist,Glist,Slist);
end
``````

## Assignment Code

``````
%*** CHAPTER 6: INVERSE KINEMATICS ***

function [thetalist, success] = IKinBody(Blist,M,T,thetalist0,eomg,ev)
% Takes Blist: The joint screw axes in the end-effector frame when the
%              manipulator is at the home position,
%       M: The home configuration of the end-effector,
%       T: The desired end-effector configuration Tsd,
%       thetalist0: An initial guess of joint angles that are close to
%                   satisfying Tsd,
%       eomg: A small positive tolerance on the end-effector orientation
%             error. The returned joint angles must give an end-effector
%             orientation error less than eomg,
%       ev: A small positive tolerance on the end-effector linear position
%           error. The returned joint angles must give an end-effector
%           position error less than ev.
% Returns thetalist: Joint angles that achieve T within the specified
%                    tolerances,
%         success: A logical value where TRUE means that the function found
%                  a solution and FALSE means that it ran through the set
%                  number of maximum iterations without finding a solution
%                  within the tolerances eomg and ev.
% Uses an iterative Newton-Raphson root-finding method.
% The maximum number of iterations before the algorithm is terminated has
% been hardcoded in as a variable called maxiterations. It is set to 20 at
% the start of the function, but can be changed if needed.
% Example Inputs:
%{
clear;clc;
Blist = [[0; 0; -1; 2; 0; 0], [0; 0; 0; 0; 1; 0], [0; 0; 1; 0; 0; 0.1]];
M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]];
T = [[0, 1, 0, -5]; [1, 0, 0, 4]; [0, 0, -1, 1.6858]; [0, 0, 0, 1]];
thetalist0 = [1.5; 2.5; 3];
eomg = 0.01;
ev = 0.001;
[thetalist, success] = IKinBody(Blist,M,T,thetalist0,eomg,ev)
%}
% Output:
% thetalist =
%    1.5707
%    2.9997
%    3.1415
% success =
%     1

thetalist = thetalist0;
i = 0;
maxiterations = 20;
Vb = se3ToVec(MatrixLog6(TransInv(FKinBody(M,Blist,thetalist)) * T));
err = norm(Vb(1:3)) > eomg || norm(Vb(4:6)) > ev;
while err && i < maxiterations
thetalist = thetalist + pinv(JacobianBody(Blist,thetalist)) * Vb;
i = i + 1;
Vb = se3ToVec(MatrixLog6(TransInv(FKinBody(M,Blist,thetalist)) * T));
err = norm(Vb(1:3)) > eomg || norm(Vb(4:6)) > ev;
end
success = ~ err;
end
``````

## Assignment Code

``````
%*** CHAPTER 6: INVERSE KINEMATICS ***

function [thetalist, success] = IKinSpace(Slist,M,T,thetalist0,eomg,ev)
% Takes Slist: The joint screw axes in the space frame when the manipulator
%              is at the home position,
%       M: The home configuration of the end-effector,
%       T: The desired end-effector configuration Tsd,
%       thetalist0: An initial guess of joint angles that are close to
%                   satisfying Tsd,
%       eomg: A small positive tolerance on the end-effector orientation
%             error. The returned joint angles must give an end-effector
%             orientation error less than eomg,
%       ev: A small positive tolerance on the end-effector linear position
%           error. The returned joint angles must give an end-effector
%           position error less than ev.
% Returns thetalist: Joint angles that achieve T within the specified
%                    tolerances,
%         success: A logical value where TRUE means that the function found
%                  a solution and FALSE means that it ran through the set
%                  number of maximum iterations without finding a solution
%                  within the tolerances eomg and ev.
% Uses an iterative Newton-Raphson root-finding method.
% The maximum number of iterations before the algorithm is terminated has
% been hardcoded in as a variable called maxiterations. It is set to 20 at
% the start of the function, but can be changed if needed.
% Example Inputs:
%{
clear;clc;
Slist = [[0; 0;  1;  4; 0;    0], ...
[0; 0;  0;  0; 1;    0], ...
[0; 0; -1; -6; 0; -0.1]];
M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]];
T = [[0, 1, 0, -5]; [1, 0, 0, 4]; [0, 0, -1, 1.6858]; [0, 0, 0, 1]];
thetalist0 = [1.5; 2.5; 3];
eomg = 0.01;
ev = 0.001;
[thetalist, success] = IKinSpace(Slist,M,T,thetalist0,eomg,ev)
%}
% Output:
% thetalist =
%    1.5707
%    2.9997
%    3.1415
% success =
%     1

thetalist = thetalist0;
i = 0;
maxiterations = 20;
Tsb = FKinSpace(M,Slist,thetalist);
Vs = Adjoint(Tsb) * se3ToVec(MatrixLog6(TransInv(Tsb) * T));
err = norm(Vs(1:3)) > eomg || norm(Vs(4:6)) > ev;
while err && i < maxiterations
thetalist = thetalist + pinv(JacobianSpace(Slist,thetalist)) * Vs;
i = i + 1;
Tsb = FKinSpace(M,Slist,thetalist);
Vs = Adjoint(Tsb) * se3ToVec(MatrixLog6(TransInv(Tsb) * T));
err = norm(Vs(1:3)) > eomg || norm(Vs(4:6)) > ev;
end
success = ~ err;
end
``````

## Assignment Code

``````
%*** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***

function taulist = InverseDynamics(thetalist,dthetalist,ddthetalist,g, ...
Ftip,Mlist,Glist,Slist)
% Takes thetalist: n-vector of joint variables,
%       dthetalist: n-vector of joint rates,
%       ddthetalist: n-vector of joint accelerations,
%       g: Gravity vector g,
%       Ftip: Spatial force applied by the end-effector expressed in frame
%             {n+1},
%       Mlist: List of link frames {i} relative to {i-1} at the home
%              position,
%       Glist: Spatial inertia matrices Gi of the links,
%       Slist: Screw axes Si of the joints in a space frame.
% Returns taulist: The n-vector of required joint forces/torques.
% This function uses forward-backward Newton-Euler iterations to solve the
% equation:
% taulist = Mlist(thetalist)ddthetalist + c(thetalist,dthetalist) ...
%           + g(thetalist) + Jtr(thetalist)Ftip
% Example Input (3 Link Robot):
%{
clear;clc;
thetalist = [0.1; 0.1; 0.1];
dthetalist = [0.1; 0.2; 0.3];
ddthetalist = [2; 1.5; 1];
g = [0; 0; -9.8];
Ftip = [1; 1; 1; 1; 1; 1];
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]];
G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
Glist = cat(3,G1,G2,G3);
Mlist = cat(4,M01,M12,M23,M34);
Slist = [[1; 0; 1;      0; 1;     0], ...
[0; 1; 0; -0.089; 0;     0], ...
[0; 1; 0; -0.089; 0; 0.425]];
taulist = InverseDynamics(thetalist,dthetalist,ddthetalist,g,Ftip, ...
Mlist,Glist,Slist)
%}
% Output:
% taulist =
%   74.6962
%  -33.0677
%   -3.2306

n = size(thetalist,1);
Mi = eye(4);
Ai = zeros(6,n);
Vi = zeros(6,n + 1);
Vdi = zeros(6,n + 1);
Vdi(4:6,1) = -g;
Fi = Ftip;
taulist = zeros(n,1);
for i=1:n
Mi = Mi * Mlist(:,:,i);
* TransInv(Mlist(:,:,i)));
Vi(:,i + 1) = AdTi(:,:,i) * Vi(:,i) + Ai(:,i) * dthetalist(i);
Vdi(:,i + 1) = AdTi(:,:,i) * Vdi(:,i) + Ai(:,i) * ddthetalist(i) ...
+ ad(Vi(:,i + 1)) * Ai(:,i) * dthetalist(i) ;
end
for i = n:-1:1
Fi = AdTi(:,:,i + 1)'* Fi + Glist(:,:,i) * Vdi(:,i + 1) ...
- ad(Vi(:,i + 1))' * (Glist(:,:,i) * Vi(:,i + 1));
taulist(i) = Fi' * Ai(:,i);
end
end
``````

## Assignment Code

``````
%*** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***

function taumat ...
= InverseDynamicsTrajectory(thetamat,dthetamat,ddthetamat,g, ...
Ftipmat,Mlist,Glist,Slist)
% Takes thetamat: An N x n matrix of robot joint variables,
%       dthetamat: An N x n matrix of robot joint velocities,
%       ddthetamat: An N x n matrix of robot joint accelerations,
%       g: Gravity vector g,
%       Ftipmat: An N x 6 matrix of spatial forces applied by the
%                end-effector (If there are no tip forces, the user should
%                input a zero and a zero matrix will be used),
%       Mlist: List of link frames i relative to i-1 at the home position,
%       Glist: Spatial inertia matrices Gi of the links,
%       Slist: Screw axes Si of the joints in a space frame.
% Returns taumat: The N x n matrix of joint forces/torques for the
%                 specified trajectory, where each of the N rows is the
%                 vector of joint forces/torques at each time step.
% This function uses InverseDynamics to calculate the joint forces/torques
% required to move the serial chain along the given trajectory.
% Example Inputs (3 Link Robot)
%{
clc;clear;
%Create a trajectory to follow using functions from Chapter 9
thetastart = [0; 0; 0];
thetaend = [pi/2; pi/2; pi/2];
Tf = 3;
N= 1000;
method = 5 ;
traj = JointTrajectory(thetastart,thetaend,Tf,N,method);
thetamat = traj;
dthetamat = zeros(1000,3);
ddthetamat = zeros(1000,3);
dt = Tf / (N-1);
for i = 1:N - 1
dthetamat(i + 1,:) = (thetamat(i + 1,:)-thetamat(i,:)) / dt;
ddthetamat(i + 1,:) = (dthetamat(i + 1,:)-dthetamat(i,:)) / dt;
end
%Initialise robot descripstion (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]];
G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
Glist = cat(3,G1,G2,G3);
Mlist = cat(4,M01,M12,M23,M34);
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);
%Output using matplotlib to plot the joint forces/torques
time=0:dt:Tf;
plot(time,taumat(:,1),'b')
hold on
plot(time,taumat(:,2),'g')
plot(time,taumat(:,3),'r')
title('Plot for Torque Trajectories')
xlabel('Time')
ylabel('Torque')
legend('Tau1','Tau2','Tau3')
%}

thetamat = thetamat';
dthetamat = dthetamat';
ddthetamat = ddthetamat';
Ftipmat = Ftipmat';
taumat = thetamat;
for i = 1:size(thetamat,2)
taumat(:,i) ...
= InverseDynamics(thetamat(:,i),dthetamat(:,i),ddthetamat(:,i),g, ...
Ftipmat(:,i),Mlist,Glist,Slist);
end
taumat = taumat';
end
``````

## Assignment Code

``````
%*** CHAPTER 5: VELOCITY KINEMATICS AND STATICS ***

function Jb = JacobianBody(Blist,thetalist)
% Takes Blist: The joint screw axes in the end-effector frame when the
%              manipulator is at the home position,
%       thetalist: A list of joint coordinates.
% Returns the corresponding body Jacobian (6xn real numbers).
% Example Input:
%{
clear;clc;
Blist = [[0; 0; 1;   0; 0.2; 0.2], ...
[1; 0; 0;   2;   0;   3], ...
[0; 1; 0;   0;   2;   1], ...
[1; 0; 0; 0.2; 0.3; 0.4]];
thetalist = [0.2; 1.1; 0.1; 1.2];
Jb = JacobianBody(Blist, thetalist)
%}
% Output:
% Jb =
%   -0.0453    0.9950         0    1.0000
%    0.7436    0.0930    0.3624         0
%   -0.6671    0.0362   -0.9320         0
%    2.3259    1.6681    0.5641    0.2000
%   -1.4432    2.9456    1.4331    0.3000
%   -2.0664    1.8288   -1.5887    0.4000

Jb = Blist;
T = eye(4);
for i = length(thetalist) - 1:-1:1
T = T * MatrixExp6(VecTose3(-1 * Blist(:,i + 1) * thetalist(i + 1)));
end
end
``````

## Assignment Code

``````
%*** CHAPTER 5: VELOCITY KINEMATICS AND STATICS ***

function Js = JacobianSpace(Slist,thetalist)
% Takes Slist: The joint screw axes in the space frame when the manipulator
%              is at the home position,
%       thetalist: A list of joint coordinates.
% Returns the corresponding space Jacobian (6xn real numbers).
% Example Input:
%{
clear;clc;
Slist = [[0; 0; 1;   0; 0.2; 0.2], ...
[1; 0; 0;   2;   0;   3], ...
[0; 1; 0;   0;   2;   1], ...
[1; 0; 0; 0.2; 0.3; 0.4]];
thetalist = [0.2; 1.1; 0.1; 1.2];
Js = JacobianSpace(Slist,thetalist)
%}
% Output:
% Js =
%         0    0.9801   -0.0901    0.9575
%         0    0.1987    0.4446    0.2849
%    1.0000         0    0.8912   -0.0453
%         0    1.9522   -2.2164   -0.5116
%    0.2000    0.4365   -2.4371    2.7754
%    0.2000    2.9603    3.2357    2.2251

Js = Slist;
T = eye(4);
for i = 2:length(thetalist)
T = T * MatrixExp6(VecTose3(Slist(:,i - 1) * thetalist(i - 1)));
end
end
``````

## Assignment Code

``````
%*** CHAPTER 9: TRAJECTORY GENERATION ***

function traj = JointTrajectory(thetastart,thetaend,Tf,N,method)
% Takes thetastart: The initial joint variables,
%       thetaend: The final joint variables,
%       Tf: Total time of the motion in seconds from rest to rest,
%       N: The number of points N > 1 (Start and stop) in the discrete
%          representation of the trajectory,
%       method: The time-scaling method, where 3 indicates cubic
%               (third-order polynomial) time scaling and 5 indicates
%               quintic (fifth-order polynomial) time scaling.
% Returns traj: A trajectory as an N x n matrix, where each row is an
%               n-vector of joint variables at an instant in time. The
%               first row is thetastart and the Nth row is thetaend . The
%               elapsed time between each row is Tf/(N - 1).
% The returned trajectory is a straight-line motion in joint space.
% Example Input:
%{
clear;clc;
thetastart = [1; 0; 0; 1; 1; 0.2; 0; 1];
thetaend = [1.2; 0.5; 0.6; 1.1; 2;2; 0.9; 1];
Tf = 4;
N = 6;
method = 3;
traj = JointTrajectory(thetastart,thetaend,Tf,N,method)
%}
% Output:
% traj =
%   1.0000        0        0   1.0000   1.0000   0.2000        0   1.0000
%   1.0208   0.0520   0.0624   1.0104   1.1040   0.3872   0.0936   1.0000
%   1.0704   0.1760   0.2112   1.0352   1.3520   0.8336   0.3168   1.0000
%   1.1296   0.3240   0.3888   1.0648   1.6480   1.3664   0.5832   1.0000
%   1.1792   0.4480   0.5376   1.0896   1.8960   1.8128   0.8064   1.0000
%   1.2000   0.5000   0.6000   1.1000   2.0000   2.0000   0.9000   1.0000

timegap = Tf / (N - 1);
traj = zeros(size(thetastart,1),N);
for i = 1:N
if method == 3
s = CubicTimeScaling(Tf,timegap * (i - 1));
else
s = QuinticTimeScaling(Tf,timegap * (i - 1));
end
traj(:,i) = thetastart + s * (thetaend - thetastart);
end
traj = traj';
end
``````

## Assignment Code

``````
%*** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***

function M = MassMatrix(thetalist,Mlist,Glist,Slist)
% Takes thetalist: A list of joint variables,
%       Mlist: List of link frames i relative to i-1 at the home position,
%       Glist: Spatial inertia matrices Gi of the links,
%       Slist: Screw axes Si of the joints in a space frame.
% Returns M: The numerical inertia matrix M(thetalist) of an n-joint serial
%            chain at the given configuration thetalist.
% This function calls InverseDynamics n times, each time passing a
% ddthetalist vector with a single element equal to one and all other
% inputs set to zero. Each call of InverseDynamics generates a single
% column, and these columns are assembled to create the inertia matrix.
% Example Input (3 Link Robot):
%{
clear;clc;
thetalist = [0.1; 0.1; 0.1];
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]];
G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
Glist = cat(3,G1,G2,G3);
Mlist = cat(4,M01,M12,M23,M34);
Slist = [[1; 0; 1;      0; 1;     0], ...
[0; 1; 0; -0.089; 0;     0], ...
[0; 1; 0; -0.089; 0; 0.425]];
M = MassMatrix(thetalist,Mlist,Glist,Slist)
%}
% Output:
% M =
%   22.5433   -0.3071   -0.0072
%   -0.3071    1.9685    0.4322
%   -0.0072    0.4322    0.1916

n = size(thetalist,1);
M = zeros(n);
for i = 1:n
ddthetalist = zeros(n,1);
ddthetalist(i) = 1;
M(:,i) = InverseDynamics(thetalist,zeros(n,1),ddthetalist,[0; 0; 0], ...
[0; 0; 0; 0; 0; 0],Mlist,Glist,Slist);
end
end
``````

## Assignment Code

``````
%*** CHAPTER 3: RIGID-BODY MOTIONS ***

function  R = MatrixExp3(so3mat)
% Takes a 3x3 so(3) representation of exponential coordinates.
% Returns R in SO(3) that is achieved by rotating about omghat by theta
% from an initial orientation R = I.
% Example Input:
%{
clear;clc;
so3mat = [[0, -3, 2]; [3, 0, -1]; [-2, 1, 0]];
R = MatrixExp3(so3mat)
%}
% Output:
% R =
%   -0.6949    0.7135    0.0893
%   -0.1920   -0.3038    0.9332
%    0.6930    0.6313    0.3481

omgtheta = so3ToVec(so3mat);
if NearZero(norm(omgtheta))
R = eye(3);
else
[omghat, theta] = AxisAng3(omgtheta);
omgmat = so3mat / theta;
R = eye(3) + sin(theta) * omgmat + (1 - cos(theta)) * omgmat * omgmat;
end
end
``````

## Assignment Code

``````
%*** CHAPTER 3: RIGID-BODY MOTIONS ***

function T = MatrixExp6(se3mat)
% Takes a se(3) representation of exponential coordinates.
% Returns a T matrix in SE(3) that is achieved by traveling along/about the
% screw axis S for a distance theta from an initial configuration T = I.
% Example Input:
%{
clear;clc;
se3mat = [ 0,      0,       0,      0;
0,      0, -1.5708, 2.3562;
0, 1.5708,       0, 2.3562;
0,      0,       0,      0]
T = MatrixExp6(se3mat)
%}
% Output:
% T =
%    1.0000         0         0         0
%         0    0.0000   -1.0000   -0.0000
%         0    1.0000    0.0000    3.0000
%         0         0         0    1.0000

omgtheta = so3ToVec(se3mat(1:3,1:3));
if NearZero(norm(omgtheta))
T = [eye(3), se3mat(1:3,4); 0, 0, 0, 1];
else
[omghat, theta] = AxisAng3(omgtheta);
omgmat = se3mat(1:3,1:3) / theta;
T = [MatrixExp3(se3mat(1:3,1:3)), ...
(eye(3) * theta + (1 - cos(theta)) * omgmat ...
+ (theta - sin(theta)) * omgmat * omgmat) ...
* se3mat(1:3,4) / theta;
0, 0, 0, 1];
end
end
``````

## Assignment Code

``````
%*** CHAPTER 3: RIGID-BODY MOTIONS ***

function so3mat = MatrixLog3(R)
% Takes R (rotation matrix).
% Returns the corresponding so(3) representation of exponential
% coordinates.
% Example Input:
%{
clear;clc;
R = [[0, 0, 1]; [1, 0, 0]; [0, 1, 0]];
so3mat = MatrixLog3(R)
%}
% Output:
% angvmat =
%         0   -1.2092    1.2092
%    1.2092         0   -1.2092
%   -1.2092    1.2092         0

if NearZero(norm(R - eye(3)))
so3mat = zeros(3);
elseif NearZero(trace(R) + 1)
if ~NearZero(1 + R(3,3))
omg = (1 / sqrt(2 * (1 + R(3,3)))) * [R(1,3); R(2,3); 1 + R(3,3)];
elseif ~NearZero(1 + R(2,2))
omg = (1 / sqrt(2 * (1 + R(2,2)))) * [R(1,2); 1 + R(2,2); R(3,2)];
else
omg = (1 / sqrt(2 * (1 + R(1,1)))) * [1 + R(1,1); R(2,1); R(3,1)];
end
so3mat = VecToso3(pi * omg);
else
acosinput = (trace(R) - 1) / 2;
if acosinput > 1
acosinput = 1;
elseif acosinput < -1
acosinput = -1;
end
theta = acos(acosinput);
so3mat = theta * (1 / (2 * sin(theta))) * (R - R');
end
end
``````

## Assignment Code

``````
%*** CHAPTER 3: RIGID-BODY MOTIONS ***

function expmat = MatrixLog6(T)
% Takes a transformation matrix T in SE(3).
% Returns the corresponding se(3) representation of exponential
% coordinates.
% Example Input:
%{
clear;clc;
T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]];
expmat = MatrixLog6(T)
%}
% Output:
% expc6 =
%         0         0         0         0
%         0         0   -1.5708    2.3562
%         0    1.5708         0    2.3562
%         0         0         0         0

[R, p] = TransToRp(T);
if NearZero(norm(R - eye(3)))
expmat = [zeros(3), T(1:3,4); 0, 0, 0, 0];
else
acosinput = (trace(R) - 1) / 2;
if acosinput > 1
acosinput = 1;
elseif acosinput < -1
acosinput = -1;
end
theta = acos(acosinput);
omgmat = MatrixLog3(R);
expmat = [ omgmat, (eye(3) - omgmat / 2 ...
+ (1 / theta - cot(theta / 2) / 2) ...
* omgmat * omgmat / theta) * p;
0, 0, 0, 0];
end
end
``````

## Assignment Code

``````
%*** BASIC HELPER FUNCTIONS ***

function judge = NearZero(near)
% Takes a scalar.
% Checks if the scalar is small enough to be neglected.
% Example Input:
%{
clear;clc;
near = -1e-7;
judge = NearZero(near)
%}
% Output:
% judge =
%     1

judge = norm(near) < 1e-6;

``````

## Assignment Code

``````
%*** BASIC HELPER FUNCTIONS ***

function norm_v = Normalize(V)
% Takes in a vector.
% Scales it to a unit vector.
% Example Input:
%{
clear;clc;
V = [1; 2; 3];
norm_v = Normalize(V)
%}
% Output:
% norm_v =
%    0.2673
%    0.5345
%    0.8018

norm_v = V / norm(V);
end

``````

## Assignment Code

``````
%*** CHAPTER 9: TRAJECTORY GENERATION ***

function s = QuinticTimeScaling(Tf,t)
% Takes Tf: Total time of the motion in seconds from rest to rest,
%       t: The current time t satisfying 0 < t < Tf.
% Returns s: The path parameter s(t) corresponding to a fifth-order
%            polynomial motion that begins and ends at zero velocity and
%            zero acceleration.
% Example Input:
%{
clear;clc;
Tf = 2;
t = 0.6;
s = QuinticTimeScaling(Tf,t)
%}
% Output:
% s =
%    0.1631

s = 10 * (t / Tf) ^ 3 - 15 * (t / Tf) ^ 4 + 6 * (t / Tf) ^ 5;
end
``````

## Assignment Code

``````
%*** CHAPTER 3: RIGID-BODY MOTIONS ***

function invR = RotInv(R)
% Takes a 3x3 rotation matrix.
% Returns the inverse (transpose).
% Example Input:
%{
clear;clc;
R = [0, 0, 1; 1, 0, 0; 0, 1, 0];
invR = RotInv(R)
%}
% Output:
% invR =
%     0     1     0
%     0     0     1
%     1     0     0

invR = R';
end
``````

## Assignment Code

``````
%*** CHAPTER 3: RIGID-BODY MOTIONS ***

function T = RpToTrans(R,p)
% Takes rotation matrix R and position p.
% Returns the corresponding homogeneous transformation matrix T in SE(3).
% Example Input:
%{
clear;clc;
R = [[1, 0, 0]; [0, 0, -1]; [0, 1, 0]];
p = [1; 2; 5];
T = RpToTrans(R,p)
%}
% Output:
% T =
%     1     0     0     1
%     0     0    -1     2
%     0     1     0     5
%     0     0     0     1

T = [R, p; 0, 0, 0, 1];
end
``````

## Assignment Code

``````
%*** CHAPTER 3: RIGID-BODY MOTIONS ***

function S = ScrewToAxis(q,s,h)
% Takes q: a point lying on the screw axis,
%       s: a unit vector in the direction of the screw axis,
%       h: the pitch of the screw axis.
% Returns the corresponding normalized screw axis.
% Example Input:
%{
clear;clc;
q = [3; 0; 0];
s = [0; 0; 1];
h = 2;
S = ScrewToAxis(q,s,h)
%}
% Output:
% S =
%     0
%     0
%     1
%     0
%    -3
%     2

S = [s; cross(q,s) + h * s];
end
``````

## Assignment Code

``````
%*** CHAPTER 9: TRAJECTORY GENERATION ***

function traj = ScrewTrajectory(Xstart,Xend,Tf,N,method)
% Takes Xstart: The initial end-effector configuration,
%       Xend: The final end-effector configuration,
%       Tf: Total time of the motion in seconds from rest to rest,
%       N: The number of points N > 1 (Start and stop) in the discrete
%          representation of the trajectory,
%       method: The time-scaling method, where 3 indicates cubic
%               (third-order polynomial) time scaling and 5 indicates
%               quintic (fifth-order polynomial) time scaling.
% Returns traj: The discretized trajectory as a list of N matrices in SE(3)
%               separated in time by Tf/(N-1). The first in the list is
%               Xstart and the Nth is Xend .
% This function calculates a trajectory corresponding to the screw motion
% about a space screw axis.
% Example Input:
%{
clear;clc;
Xstart = [[1 ,0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 1]; [0, 0, 0, 1]];
Xend = [[0, 0, 1, 0.1]; [1, 0, 0, 0]; [0, 1, 0, 4.1]; [0, 0, 0, 1]];
Tf = 5;
N = 4;
method = 3;
traj = ScrewTrajectory(Xstart,Xend,Tf,N,method)
%}
% Output:
% traj =
%    1.0000         0         0    1.0000
%         0    1.0000         0         0
%         0         0    1.0000    1.0000
%         0         0         0    1.0000
%
%    0.9041   -0.2504    0.3463    0.4410
%    0.3463    0.9041   -0.2504    0.5287
%   -0.2504    0.3463    0.9041    1.6007
%         0         0         0    1.0000
%
%    0.3463   -0.2504    0.9041   -0.1171
%    0.9041    0.3463   -0.2504    0.4727
%   -0.2504    0.9041    0.3463    3.2740
%         0         0         0    1.0000
%
%   -0.0000    0.0000    1.0000    0.1000
%    1.0000   -0.0000    0.0000   -0.0000
%    0.0000    1.0000   -0.0000    4.1000
%         0         0         0    1.0000

timegap = Tf / (N - 1);
traj = cell(1,N);
for i = 1:N
if method == 3
s = CubicTimeScaling(Tf,timegap * (i - 1));
else
s = QuinticTimeScaling(Tf,timegap * (i - 1));
end
traj{i} = Xstart * MatrixExp6(MatrixLog6(TransInv(Xstart) * Xend) * s);
end
end

``````

## Assignment Code

``````
%*** CHAPTER 3: RIGID-BODY MOTIONS ***

function V = se3ToVec(se3mat)
% Takes se3mat a 4x4 se(3) matrix
% Returns the corresponding 6-vector (representing spatial velocity).
% Example Input:
%{
clear;clc;
se3mat = [[0, -3, 2, 4]; [3, 0, -1, 5]; [-2, 1, 0, 6]; [0, 0, 0, 0]];
V = se3ToVec(se3mat)
%}
% Output:
% V =
%     1
%     2
%     3
%     4
%     5
%     6

V = [se3mat(3,2); se3mat(1,3); se3mat(2,1); se3mat(1:3,4)];
end
``````

## Assignment Code

``````
%*** CHAPTER 11: ROBOT CONTROL ***

function [taumat, thetamat] ...
= SimulateControl(thetalist,dthetalist,g,Ftipmat,Mlist,Glist, ...
Slist,thetamatd,dthetamatd,ddthetamatd, ...
gtilde,Mtildelist,Gtildelist,Kp,Ki,Kd,dt,intRes)
% Takes thetalist: n-vector of initial joint variables,
%       dthetalist: n-vector of initial joint velocities,
%       g: Actual gravity vector g,
%       Ftipmat: An N x 6 matrix of spatial forces applied by the
%                end-effector (If there are no tip forces, the user should
%                input a zero and a zero matrix will be used),
%       Mlist: Actual list of link frames i relative to i? at the home
%              position,
%       Glist: Actual spatial inertia matrices Gi of the links,
%       Slist: Screw axes Si of the joints in a space frame,
%       thetamatd: An Nxn matrix of desired joint variables from the
%                  reference trajectory,
%       dthetamatd: An Nxn matrix of desired joint velocities,
%       ddthetamatd: An Nxn matrix of desired joint accelerations,
%       gtilde: The gravity vector based on the model of the actual robot
%               (actual values given above),
%       Mtildelist: The link frame locations based on the model of the
%                   actual robot (actual values given above),
%       Gtildelist: The link spatial inertias based on the model of the
%                   actual robot (actual values given above),
%       Kp: The feedback proportional gain (identical for each joint),
%       Ki: The feedback integral gain (identical for each joint),
%       Kd: The feedback derivative gain (identical for each joint),
%       dt: The timestep between points on the reference trajectory.
%       intRes: Integration resolution is the number of times integration
%               (Euler) takes places between each time step. Must be an
%               integer value greater than or equal to 1.
% Returns taumat: An Nxn matrix of the controller commanded joint
%                 forces/torques, where each row of n forces/torques
%                 corresponds to a single time instant,
%         thetamat: An Nxn matrix of actual joint angles.
% The end of this function plots all the actual and desired joint angles.
% Example Usage
%{
clc;clear;
thetalist = [0.1; 0.1; 0.1];
dthetalist = [0.1; 0.2; 0.3];
%Initialize robot description (Example with 3 links)
g = [0; 0; -9.8];
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]];
G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
Glist = cat(3,G1,G2,G3);
Mlist = cat(4,M01,M12,M23,M34);
Slist = [[1; 0; 1;      0; 1;     0], ...
[0; 1; 0; -0.089; 0;     0], ...
[0; 1; 0; -0.089; 0; 0.425]];
dt = 0.01;
%Create a trajectory to follow
thetaend =[pi / 2; pi; 1.5 * pi];
Tf = 1;
N = Tf / dt;
method = 5;
thetamatd = JointTrajectory(thetalist,thetaend,Tf,N,method);
dthetamatd = zeros(N,3);
ddthetamatd = zeros(N,3);
dt = Tf / (N-1);
for i = 1:N - 1
dthetamatd(i + 1,:) = (thetamatd(i + 1,:)-thetamatd(i,:)) / dt;
ddthetamatd(i + 1,:) = (dthetamatd(i + 1,:)-dthetamatd(i,:)) / dt;
end
%Possibly wrong robot description (Example with 3 links)
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]];
Ghat1 = diag([0.1, 0.1, 0.1, 4, 4, 4]);
Ghat2 = diag([0.3, 0.3, 0.1, 9, 9, 9]);
Ghat3 = diag([0.1, 0.1, 0.1, 3, 3, 3]);
Gtildelist = cat(3,Ghat1,Ghat2,Ghat3);
Mtildelist = cat(4,Mhat01,Mhat12,Mhat23,Mhat34);
Ftipmat = ones(N,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);
%}

Ftipmat = Ftipmat';
thetamatd = thetamatd';
dthetamatd = dthetamatd';
ddthetamatd = ddthetamatd';
n = size(thetamatd,2);
taumat = zeros(size(thetamatd));
thetamat = zeros(size(thetamatd));
thetacurrent = thetalist;
dthetacurrent = dthetalist;
eint = zeros(size(thetamatd,1),1);
for i=1:n
taulist ...
= ComputedTorque(thetacurrent,dthetacurrent,eint,gtilde,Mtildelist, ...
Gtildelist,Slist,thetamatd(:,i),dthetamatd(:,i), ...
ddthetamatd(:,i),Kp,Ki,Kd);
for j=1:intRes
ddthetalist ...
= ForwardDynamics(thetacurrent,dthetacurrent,taulist,g, ...
Ftipmat(:,i),Mlist,Glist,Slist);
[thetacurrent, dthetacurrent] ...
= EulerStep(thetacurrent,dthetacurrent,ddthetalist,(dt/intRes));
end
taumat(:,i) = taulist;
thetamat(:,i) = thetacurrent;
eint = eint + (dt*(thetamatd(:,i) - thetacurrent));
end
%Output using matplotlib
time=0:dt:((dt*n)-(dt));
timed=0:(dt):((dt*n)-(dt));
figure
hold on
col = rand(1,3);
plot(time,(thetamat(i,:)'),'-','Color',col)
plot(timed,(thetamatd(i,:)'),'.','Color',col)
leg{(2*i)-1} = (strcat('ActualTheta', num2str(i)));
leg{(2*i)} = (strcat('DesiredTheta', num2str(i)));
end
title('Plot of Actual and Desired Joint Angles')
xlabel('Time')
ylabel('Joint Angles')
legend(leg, 'Location', 'NorthWest')
taumat = taumat';
thetamat = thetamat';
end
``````

## Assignment Code

``````
%*** CHAPTER 3: RIGID-BODY MOTIONS ***

function omg = so3ToVec(so3mat)
% Takes a 3x3 skew-symmetric matrix (an element of so(3)).
% Returns the corresponding 3-vector (angular velocity).
% Example Input:
%{
clear;clc;
so3mat = [[0, -3, 2]; [3, 0, -1]; [-2, 1, 0]];
omg = so3ToVec(so3mat)
%}
% Output:
% omg =
%     1
%     2
%     3

omg = [so3mat(3,2); so3mat(1,3); so3mat(2,1)];
end
``````

## Assignment Code

``````
%*** CHAPTER 3: RIGID-BODY MOTIONS ***

function invT = TransInv(T)
% Takes a transformation matrix T.
% Returns its inverse.
% Uses the structure of transformation matrices to avoid taking a matrix
% inverse, for efficiency.
% Example Input:
%{
clear;clc;
T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]];
invT = TransInv(T)
%}
% Ouput:
% invT =
%     1     0     0     0
%     0     0     1    -3
%     0    -1     0     0
%     0     0     0     1

[R, p] = TransToRp(T);
invT = [R', -R' * p; 0, 0, 0, 1];
end
``````

## Assignment Code

``````
%*** CHAPTER 3: RIGID-BODY MOTIONS ***

function  [R, p] = TransToRp(T)
% Takes the transformation matrix T in SE(3)
% Returns R: the corresponding rotation matrix
%         p: the corresponding position vector .
% Example Input:
%{
clear;clc;
T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]];
[R, p] = TransToRp(T)
%}
% Output:
% R =
%     1     0     0
%     0     0    -1
%     0     1     0
% p =
%     0
%     0
%     3

R = T(1:3,1:3);
p = T(1:3,4);
end
``````

## Assignment Code

``````
%*** CHAPTER 3: RIGID-BODY MOTIONS ***

function se3mat = VecTose3(V)
% Takes a 6-vector (representing a spatial velocity).
% Returns the corresponding 4x4 se(3) matrix.
% Example Input:
%{
clear;clc;
V = [1; 2; 3; 4; 5; 6];
se3mat = VecTose3(V)
%}
% Output:
% se3mat =
%     0    -3     2     4
%     3     0    -1     5
%    -2     1     0     6
%     0     0     0     0

se3mat = [VecToso3(V(1:3)), V(4:6); 0, 0, 0, 0];
end
``````

## Assignment Code

``````
%*** CHAPTER 3: RIGID-BODY MOTIONS ***

function so3mat = VecToso3(omg)
% Takes a 3-vector (angular velocity).
% Returns the skew symmetric matrix in so(3).
% Example Input:
%{
clear;clc;
omg = [1; 2; 3];
so3mat = VecToso3(omg)
%}
% Output:
% so3mat =
%     0    -3     2
%     3     0    -1
%    -2     1     0

so3mat = [0, -omg(3), omg(2); omg(3), 0, -omg(1); -omg(2), omg(1), 0];
end
``````

## Assignment Code

``````
%*** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***

% Takes thetalist: A list of joint variables,
%       dthetalist: A list of joint rates,
%       Mlist: List of link frames i relative to i-1 at the home position,
%       Glist: Spatial inertia matrices Gi of the links,
%       Slist: Screw axes Si of the joints in a space frame.
% Returns c: The vector c(thetalist,dthetalist) of Coriolis and centripetal
%            terms for a given thetalist and dthetalist.
% This function calls InverseDynamics with g = 0, Ftip = 0, and
% ddthetalist = 0.
% Example Input (3 Link Robot):
%{
clear;clc;
thetalist = [0.1; 0.1; 0.1];
dthetalist = [0.1; 0.2; 0.3];
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]];
G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
Glist = cat(3,G1,G2,G3);
Mlist = cat(4,M01,M12,M23,M34);
Slist = [[1; 0; 1;      0; 1;     0], ...
[0; 1; 0; -0.089; 0;     0], ...
[0; 1; 0; -0.089; 0; 0.425]];
%}
% Output:
% c =
%    0.2645
%   -0.0551
%   -0.0069

c = InverseDynamics(thetalist,dthetalist,zeros(size(thetalist,1),1), ...
[0; 0; 0],[0; 0; 0; 0; 0; 0],Mlist,Glist,Slist);
end
``````

Is it free to get my assignment evaluated?

Yes. No hidden fees. You pay for the solution only, and all the explanations about how to run it are included in the price. It takes up to 24 hours to get a quote from an expert. In some cases, we can help you faster if an expert is available, but you should always order in advance to avoid the risks. You can place a new order here.

How much does it cost?

The cost depends on many factors: how far away the deadline is, how hard/big the task is, if it is code only or a report, etc. We try to give rough estimates here, but it is just for orientation (in USD):

 Regular homework \$20 - \$150 Advanced homework \$100 - \$300 Group project or a report \$200 - \$500 Mid-term or final project \$200 - \$800 Live exam help \$100 - \$300 Full thesis \$1000 - \$3000

How do I pay?

Credit card or PayPal. You don't need to create/have a Payal account in order to pay by a credit card. Paypal offers you "buyer's protection" in case of any issues.

Why do I need to pay in advance?

We have no way to request money after we send you the solution. PayPal works as a middleman, which protects you in case of any disputes, so you should feel safe paying using PayPal.

Do you do essays?

No, unless it is a data analysis essay or report. This is because essays are very personal and it is easy to see when they are written by another person. This is not the case with math and programming.

Why there are no discounts?

It is because we don't want to lie - in such services no discount can be set in advance because we set the price knowing that there is a discount. For example, if we wanted to ask for \$100, we could tell that the price is \$200 and because you are special, we can do a 50% discount. It is the way all scam websites operate. We set honest prices instead, so there is no need for fake discounts.

Do you do live tutoring?

No, it is simply not how we operate. How often do you meet a great programmer who is also a great speaker? Rarely. It is why we encourage our experts to write down explanations instead of having a live call. It is often enough to get you started - analyzing and running the solutions is a big part of learning.

What happens if I am not satisfied with the solution?

Another expert will review the task, and if your claim is reasonable - we refund the payment and often block the freelancer from our platform. Because we are so harsh with our experts - the ones working with us are very trustworthy to deliver high-quality assignment solutions on time.

## Popular Solved Assignments Like This

Customer Feedback

"Thanks for explanations after the assignment was already completed... Emily is such a nice tutor! "

Order #13073

Find Us On