(* ::Package:: *)

(* ::Title::Closed:: *)
(*Modern Robotics: Mechanics, Planning, and Control*)
(*Code Library*)


(* ::Text:: *)
(* :Name: ModernRobotics` *)
(* :Book: Modern Robotics: Mechanics, Planning, and Control *)
(* :Author: Huan Weng, Jarvis Schultz, Mikhail Todes*)
(* :Contact: huanweng@u.northwestern.edu*)
(* :Summary: This package is the code library accompanying the book. *)
(* :Package Version: 1.1.0*)
(* :Mathematica Version: 11.3 *)
(* Tested in Mathematica 11.3 *)


BeginPackage["ModernRobotics`"];


(* ::Section::Closed:: *)
(*USAGE*)


(* ::Subsection::Closed:: *)
(*BASIC HELPER FUNCTIONS*)


NearZero::usage=
"NearZero[s]:
Takes a scalar.
Checks if the scalar is small enough to be neglected.

Example:
	Input: 
		judge = NearZero[-10^-7]
	Output:	
		True"


(* ::Subsection::Closed:: *)
(*CHAPTER 3: RIGID-BODY MOTION*)


RotInv::usage=
"RotInv[R]:
Takes a 3x3 rotation matrix.
Returns the inverse (transpose).

Example:
	Input: 
		invR = RotInv[{{0,0,1},{1,0,0},{0,1,0}}]
	Output: 
		{{0,1,0},{0,0,1},{1,0,0}}"


VecToso3::usage=
"VecToso3[omg]:
Takes a 3-vector (angular velocity).
Returns the skew symmetric matrix in so3.

Example:
	Input: 
		so3mat = VecToso3[{{1},{2},{3}}]
	Output: 
		{{0,-3,2},{3,0,-1},{-2,1,0}}"


so3ToVec::usage=
"so3ToVec[so3mat]:
Takes a 3x3 skew-symmetric matrix (an element of so(3)).
Returns the corresponding vector (angular velocity).

Example:
	Input: 
		omg = so3ToVec[{{0,-3,2},{3,0,-1},{-2,1,0}}]
	Output: 
		{{1},{2},{3}}"


AxisAng3::usage=
"AxisAng3[expc3]:
	Takes A 3-vector of exponential coordinates for rotation.
	Returns unit rotation axis omghat and the corresponding rotation angle
	theta.
	The first element of the output is the unit rotation axis column vector 
	(omghat) and the second element is theta.

Example:
	Input: 
		{omghat,theta} = AxisAng3[{{1,2,3}}\[Transpose]]//N
	Output: 
		{{{0.2672612419124244},{0.5345224838248488},{0.8017837257372732}},
		 3.7416573867739413}"


MatrixExp3::usage=
"MatrixExp3[so3mat]:
Takes a 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: 
		R = MatrixExp3[{{0,-3,2},{3,0,-1},{-2,1,0}}]//N
	Output: 
		{{-0.694921,0.713521,0.0892929},{-0.192007,-0.303785,0.933192},
		 {0.692978,0.63135,0.348107}}"


MatrixLog3::usage=
"MatrixLog3[R]:
Takes R (rotation matrix).
Returns the corresponding so(3) representation of exponential coordinates.

Example:
	Input: 
		so3mat = MatrixLog3[{{0,0,1},{1,0,0},{0,1,0}}]//N
	Output: 
		{{0,-1.2092,1.2092},{1.2092,0,-1.2092},{-1.2092,1.2092,0}}"


DistanceToSO3::usage=
"DistanceToSO3[mat]:
Takes mat as a 3x3 matrix.
Returns the Frobenius norm to describe the distance of mat from the SO(3) 
manifold.
Computes the distance from mat to the SO(3) manifold using the following 
method: If det(mat) <= 0, return a large number. If det(mat) > 0, return 
norm(mat'.mat - I).

Example:
	Input: 
		d = DistanceToSO3[{{1.0,0.0,0.0},{0.0,0.1,-0.95},{0.0,1.0,0.1}}]
	Output: 
		0.088353"


TestIfSO3::usage=
"TestIfSO3[mat]:
Takes mat as a 3x3 matrix.
Check if mat is close to or on the manifold SO(3).

Example:
	Input: 
		judge = TestIfSO3[{{1.0,0.0,0.0},{0.0,0.1,-0.95},{0.0,1.0,0.1}}]
	Output: 
		False"


ProjectToSO3::usage=
"ProjectToSO3[mat]:
Takes a matrix near SO(3) to project to SO(3).
Returns the closest rotation matrix that is in SO(3).
This function uses singular-value decomposition (see
http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review)
and is only appropriate for matrices close to SO(3).

Example:
	Input: 
		R = ProjectToSO3[{{0.675,0.150,0.720},{0.370,0.771,-0.511},
						  {-0.630,0.619,0.472}}]//N
	Output: 
		{{0.679011,0.148945,0.718859},{0.373207,0.773196,-0.512723}, 
		 {-0.632187,0.616428,0.469421}}"


RpToTrans::usage=
"RpToTrans[R,p]:
Takes rotation matrix R and position p.
Returns corresponding homogeneous transformation matrix T in SE(3).

Example:
	Input: 
		T = RpToTrans[{{1,0,0},{0,0,-1},{0,1,0}},{{1,2,5}}\[Transpose]]
	Output: 
		{{1, 0, 0, 1}, {0, 0, -1, 2}, {0, 1, 0, 5}, {0, 0, 0, 1}}"


TransToRp::usage=
"TransToRp[T]:
Takes transformation matrix T in SE(3).
Returns 
	R: the corresponding rotation matrix,
	p: the corresponding position vector.
The first element of the output is the rotation matrix and the second is the
position vector.

Example:
	Input: 
		{R,p} = TransToRp[{{1,0,0,0},{0,0,-1,0},{0,1,0,3},{0,0,0,1}}]
	Output: 
		{{{1,0,0},{0,0,-1},{0,1,0}},{{0},{0},{3}}}"


TransInv::usage=
"TransInv[T]:
Takes T a transformation matrix.
Returns its inverse.
Used the structure of transformation matrices to avoid taking a matrix
inverse, for efficiency.

Example:
	Input: 
		invT = TransInv[{{1,0,0,0},{0,0,-1,0},{0,1,0,3},{0,0,0,1}}]
	Output: 
		{{1,0,0,0},{0,0,1,-3}, {0,-1,0,0},{0,0,0,1}}"


VecTose3::usage=
"VecTose3[V]:
Takes a 6-vector (representing a spatial velocity).
Returns the corresponding 4x4 se(3) matrix

Example:
	Input: 
		se3mat = VecTose3[{{1,2,3,4,5,6}}\[Transpose]]
	Output: 
		{{0,-3,2,4},{3,0,-1,5},{-2,1,0,6},{0,0,0,0}}"


se3ToVec::usage=
"se3ToVec[se3mat]:
Takes se3mat a 4x4 se(3) matrix.
Returns the corresponding 6-vector (representing spatial velocity).

Example:
	Input: 
		V = se3ToVec[{{0,-3,2,4},{3,0,-1,5},{-2,1,0,6},{0,0,0,0}}]
	Output: 
		{{1},{2},{3},{4},{5},{6}}"


Adjoint::usage=
"Adjoint[T]:
	Takes T a transformation matrix SE(3).
	Returns the corresponding 6x6 adjoint representation [AdT]

Example:
	Input: 
		AdT = Adjoint[{{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}}"


ScrewToAxis::usage=
"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: 
		S = ScrewToAxis[{{3,0,0}}\[Transpose],{{0,0,1}}\[Transpose],2]
	Output: 
		{{0},{0},{1},{0},{-3},{2}}"


AxisAng6::usage=
"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.
	The first element of the output is the screw axis S and the second is the 
	distance theta.

Example:
	Input: 
		{S, theta} = AxisAng6[{{1},{0},{0},{1},{2},{3}}]
	Output: 
		{{{1},{0},{0},{1},{2},{3}},1}"


MatrixExp6::usage=
"MatrixExp6[se3mat]:
Takes a se(3) representation of exponential coordinates.
Returns a T matrix 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: 
		T = MatrixExp6[
			{{0,0,0,0},{0,0,-1.5707963267948966,2.3561944901923448},
			 {0,1.5707963267948966,0,2.3561944901923448},{0,0,0,0}}
		]
	Output: 
		{{1,0,0,0},{0,0,-1,0},{0,1,0,3},{0,0,0,1}}"


MatrixLog6::usage=
"MatrixLog6[T]:
Takes a transformation matrix T in SE(3).
Returns the corresponding se(3) representation of exponential coordinates.

Example:
	Input: 
		se3mat = MatrixLog6[{{1,0,0,0},{0,0,-1,0},{0,1,0,3},{0,0,0,1}}]//N
	Output: 
		{{0,0,0,0},{0,0,-1.5708,2.35619},{0,1.5708,0,2.35619},{0,0,0,0}}"


DistanceToSE3::usage=
"DistanceToSE3[mat]:
Takes mat as a 4x4 matrix.
Returns the Frobenius norm to describe the distance of mat from the SE(3) 
manifold.
Computes the determinant of matR, the top 3x3 submatrix of mat. If 
det (matR) <= 0, return a large number. If det (mat) > 0, replace the top 3x3
submatrix of mat with matR'.matR, and set the first three entries of the 
fourth column of mat to zero. Then return norm (mat - I).

Example :
	Input: 
		d = DistanceToSE3[{{1.0,0.0,0.0,1.2},{0.0,0.1,-0.95,1.5},
						   {0.0,1.0,0.1,-0.9},{0.0,0.0,0.1,0.98}}]
	Output: 
		0.134931"


TestIfSE3::usage=
"TestIfSE3[mat]:
Takes mat as a 4X4 matrix.
Check if mat is close to or on the manifold SE(3).

Example:
	Input: 
		judge = TestIfSE3[{{1.0,0.0,0.0,1.2},{0.0,0.1,-0.95,1.5},
						   {0.0,1.0,0.1,-0.9},{0.0,0.0,0.1,0.98}}]
	Output: 
		False"


ProjectToSE3::usage=
"ProjectToSE3[mat]:
Takes a matrix near SE(3) to project to SE(3).
Returns the closest rotation matrix that is in SE(3).
This function uses singular-value decomposition (see
http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review)
and is only appropriate for matrices close to SE(3).

Example:
	Input: 
		T = ProjectToSE3[{{0.675,0.150,0.720,1.2},
						  {0.370,0.771,-0.511,5.4},
						  {-0.630,0.619,0.472,3.6},
						  {0.003,0.002,0.010,0.9}}]
	Output: 
		{{0.679011,0.148945,0.718859,1.2},{0.373207,0.773196,-0.512723,5.4},
		 {-0.632187,0.616428,0.469421,3.6},{0,0,0,1}}"


(* ::Subsection::Closed:: *)
(*CHAPTER 4: FORWARD KINEMATICS*)


FKinBody::usage=
"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, in the format of a matrix 
		   with axes as the columns,
	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:
		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}}\[Transpose];
		thetalist = {{(Pi/2.0),3,Pi}}\[Transpose];
		T = FKinBody[M,Blist,thetalist]
	Output:
		{{0,1,0,-5},{1,0,0,4},{0,0,-1,1.68584},{0,0,0,1}}"


FKinSpace::usage=
"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, in the format of a matrix with screw axes as
		   the columns,
	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 Fixed Space Frame).

Example:
	Inputs:
		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}}\[Transpose];
		thetalist = {{Pi/2,3,Pi}}\[Transpose];
		T = FKinSpace[M,Slist,thetalist]
	Output:
		{{0,1,0,-5},{1,0,0,4},{0,0,-1,1.68584},{0,0,0,1}}"


(* ::Subsection::Closed:: *)
(*CHAPTER 5: VELOCITY KINEMATICS AND STATICS*)


JacobianBody::usage=
"JacobianBody[Blist,thetalist]:
Takes 
	Blist: The joint screw axes in the end-effector frame when the 
		   manipulator is at the home position, in the format of a matrix
		   with axes as the columns,
	thetalist:A list of joint coordinates.
Returns the corresponding body Jacobian (6xn real numbers).

Example:
	Inputs:
		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}}\[Transpose];
		thetalist = {{0.2,1.1,0.1,1.2}}\[Transpose];
		Jb = JacobianBody[Blist,thetalist]
	Output:
		{{-0.0452841,0.995004,0,1},{0.743593,0.0930486,0.362358,0},
		 {-0.667097,0.0361754,-0.932039,0},{2.32586,1.66809,0.564108,0.2},
		 {-1.44321,2.94561,1.43307,0.3},{-2.0664,1.82882,-1.58869,0.4}}"


JacobianSpace::usage=
"JacobianSpace[Slist,thetalist]:
Takes 
	Slist: The joint screw axes in the space frame when the manipulator is at
		   the home position, in the format of a matrix with axes as the 
		   columns,
	thetalist: A list of joint coordinates.
Returns the corresponding space Jacobian (6xn real numbers).

Example:
	Inputs:
		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}}\[Transpose];
		thetalist = {{0.2,1.1,0.1,1.2}}\[Transpose];
		Js = JacobianSpace[Slist, thetalist]
	Ouput:
		{{0,0.980067,-0.0901156,0.957494},{0,0.198669,0.444554,0.284876},
		{1,0,0.891207,-0.0452841},{0,1.95219,-2.21635,-0.511615},
		{0.2,0.436541,-2.43713,2.77536},{0.2,2.96027,3.23573,2.22512}}"


(* ::Subsection::Closed:: *)
(*CHAPTER 6: INVERSE KINEMATICS*)


IKinBody::usage=
"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, in the format of a matrix
		   with axes as the columns,
	M: The home configuration of the end-effector,
	T: The desired end-effector configuration T,
	thetalist0: An initial guess of joint angles that are close to satisfying
				T,
	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 tolerance, 
	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:
		Blist = {{0,0,-1,2,0,0},{0,0,0,0,1,0},{0,0,1,0,0,0.1}}\[Transpose];
		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}}\[Transpose];
		eomg = 0.01;
		ev = 0.001;
		{thetalist,success} = IKinBody[Blist,M,T,thetalist0,eomg,ev]
	Ouput:
		{{{1.57074},{2.99967},{3.14154}},True}"


IKinSpace::usage=
"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, in the format of a matrix with axes as the 
		   columns,
	M: The home configuration of the end-effector,
	T: The desired end-effector configuration T,
	thetalist0: An initial guess of joint angles that are close to satisfying
				T,
	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.

Exapmle:
	Inputs:
		Slist = {{0,0,1,4,0,0},{0,0,0,0,1,0},{0,0,-1,-6,0,-0.1}}\[Transpose];
		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}}\[Transpose];
		eomg = 0.01;
		ev = 0.001;
		{thetalist,success} = IKinSpace[Slist,M,T,thetalist0,eomg,ev]
	Ouput:
		{{{1.57074},{2.99966},{3.14153}},True}"


(* ::Subsection::Closed:: *)
(*CHAPTER 8: DYNAMICS OF OPEN CHAINS*)


ad::usage=
"ad[V]:
	Takes a 6-vector spatial velocity.
	Returns the corresponding 6x6 matrix [adV].
	Used to calculate the Lie bracket [V1, V2] = [adV1] V2

Example:
	Input: 
		mat = ad[{{1,2,3,4,5,6}}\[Transpose]]
	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}}"


InverseDynamics::usage=
"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:L ist 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, in the format of a 
		   matrix with axes as the columns.
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 (3 Link Robot):
	Inputs: 
		thetalist = {{0.1,0.1,0.1}}\[Transpose];
		dthetalist = {{0.1,0.2,0.3}}\[Transpose];
		ddthetalist = {{2,1.5,1}}\[Transpose];
		g = {{0,0,-9.8}}\[Transpose];
		Ftip = {{1,1,1,1,1,1}}\[Transpose];
		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 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}];
		G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}];
		G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095,
							 2.275,2.275,2.275}];
		Glist = {G1,G2,G3};
		Mlist = {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}}\[Transpose];
		taulist = InverseDynamics[thetalist,dthetalist,ddthetalist,g,Ftip,
								  Mlist,Glist,Slist]
	Output:
		{{74.6962},{-33.0677},{-3.23057}}"


MassMatrix::usage=
"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, in the format of a 
		   matrix with axes as the columns.
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 (3 Link Robot):
	Inputs: 
		thetalist = {{0.1,0.1,0.1}}\[Transpose];
		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 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}];
		G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}];
		G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095,
							 2.275,2.275,2.275}];
		Glist = {G1,G2,G3};
		Mlist = {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}}\[Transpose];
		M = MassMatrix[thetalist,Mlist,Glist,Slist]
	Output:
		{{22.5433,-0.307147,-0.00718426},{-0.307147,1.96851,0.432157}, 
		 {-0.00718426,0.432157,0.191631}}"


VelQuadraticForces::usage=
"VelQuadraticForces[thetalist,dthetalist,Mlist,Glist,Slist]:
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, in the format of a
		   matrix with axes as the columns.
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(3 Link Robot):
	Inputs:
		thetalist = {{0.1,0.1,0.1}}\[Transpose];
		dthetalist = {{0.1,0.2,0.3}}\[Transpose];
		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 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}];
		G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}];
		G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095,
							 2.275,2.275,2.275}];
		Glist = {G1,G2,G3};
		Mlist = {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}}\[Transpose];
		c = VelQuadraticForces[thetalist,dthetalist,Mlist,Glist,Slist]
	Output:
		{{0.264531},{-0.0550516},{-0.00689132}}"


GravityForces::usage=
"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, in the format of a
		   matrix with axes as the columns.
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 (3 Link Robot):
	Inputs: 
		thetalist = {{0.1,0.1,0.1}}\[Transpose];
		g = {{0,0,-9.8}}\[Transpose];
		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 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}];
		G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}];
		G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095,
							 2.275,2.275,2.275}];
		Glist = {G1,G2,G3};
		Mlist = {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}}\[Transpose];
		grav = GravityForces[thetalist,g,Mlist,Glist,Slist]
	Output:
		{{28.4033},{-37.6409},{-5.44159}}"


EndEffectorForces::usage=
"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, in the format of a
	       matrix with axes as the columns.
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 (3 Link Robot):
	Inputs:
		thetalist = {{0.1,0.1,0.1}}\[Transpose];
		Ftip = {{1,1,1,1,1,1}}\[Transpose];
		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 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}];
		G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}];
		G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095,
		                     2.275,2.275,2.275}];
		Glist = {G1,G2,G3};
		Mlist = {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}}\[Transpose];
		JTFtip = EndEffectorForces[thetalist,Ftip,Mlist,Glist,Slist]
	Output:
		{{1.40955}, {1.85771}, {1.39241}}"


ForwardDynamics::usage=
"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, in the format of a 
		   matrix with axes as the columns.
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 (3 Link Robot):
	Inputs: 
		thetalist = {{0.1,0.1,0.1}}\[Transpose];
		dthetalist = {{0.1,0.2,0.3}}\[Transpose];
		taulist = {{0.5,0.6,0.7}}\[Transpose];
		g = {{0,0,-9.8}}\[Transpose];
		Ftip = {{1,1,1,1,1,1}}\[Transpose];
		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 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}];
		G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}];
		G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095,
							 2.275,2.275,2.275}];
		Glist = {G1,G2,G3};
		Mlist = {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}}\[Transpose];
		ddthetalist = ForwardDynamics[thetalist,dthetalist,taulist,g,Ftip,
									  Mlist,Glist,Slist]
	Output:
		{{-0.973929}, {25.5847}, {-32.915}}"


EulerStep::usage=
"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 (3 Link Robot):
	Inputs\:ff1a
		thetalist = {{0.1,0.1,0.1}}\[Transpose];
		dthetalist = {{0.1,0.2,0.3}}\[Transpose];
		ddthetalist = {{2,1.5,1}}\[Transpose];
		dt = 0.1;
		{thetalistNext,dthetalistNext} = EulerStep[thetalist,dthetalist,
		                                           ddthetalist,dt]
	Output:
		{{{0.11},{0.12},{0.13}},{{0.3},{0.35},{0.4}}}"


InverseDynamicsTrajectory::usage=
"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, in the format of a 
		   matrix with axes as the columns.
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 (3 Link Robot): 
	Inputs:
		(*Create a trajectory to follow using functions from Chapter*)
		thetastart = {{0,0,0}}\[Transpose];
		thetaend = {{Pi/2,Pi/2,Pi/2}}\[Transpose];
		Tf = 3;
		No = 1000;
		method = 5;
		traj = JointTrajectory[thetastart,thetaend,Tf,No,method];
		thetamat = traj;
		dthetamat = traj;
		ddthetamat = traj;
		dt = Tf/(No-1.0);
		Do[
			dthetamat[[i,;;]] = (thetamat[[i,;;]]-thetamat[[i-1,;;]])/dt;
			ddthetamat[[i,;;]] = (dthetamat[[i,;;]]-dthetamat[[i-1,;;]])/dt,
			{i,2,Dimensions[traj][[1]]}
		];
		(*Initialise robot descripstion*)
		g = {{0,0,-9.8}}\[Transpose];
		Ftipmat = ConstantArray[1,{No,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 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}];
		G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}];
		G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095,
							 2.275,2.275,2.275}];
		Glist = {G1,G2,G3};
		Mlist = {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}}\[Transpose];
		taumat = InverseDynamicsTrajectory[thetamat,dthetamat,ddthetamat,g,
										   Ftipmat,Mlist,Glist,Slist];
		(*Output to plot the joint forces/torques*)
		Tau1 = Flatten[taumat[[;;,1]]];
		Tau2 = Flatten[taumat[[;;,2]]];
		Tau3 =Flatten[ taumat[[;;,3]]];
		(*PLEASE REPLACE THE SINGLE QUOTES TO DOUBLE TUOTES 
		  BEFORE RUNNING THE COMMAND BELOW*)
		ListLinePlot[
			{Tau1,Tau2,Tau3},
			PlotLegends\[Rule]{'Tau1','Tau2','Tau3'},
			AxesLabel\[Rule]{'Time','Torque'},
			PlotLabel\[Rule]'Plot of Torque Trajectories',
			PlotRange\[Rule]Full
		]"


ForwardDynamicsTrajectory::usage=
"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, in the format of a
		   matrix with axes as the columns,
	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:
		thetalist = {{0.1,0.1,0.1}}\[Transpose];
		dthetalist = {{0.1,0.2,0.3}}\[Transpose];
		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 descripstion (Example with 3 links)*)
		g = {{0,0,-9.8}}\[Transpose];
		Ftipmat = ConstantArray[1,{Dimensions[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 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}];
		G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}];
		G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095,
							 2.275,2.275,2.275}];
		Glist = {G1,G2,G3};
		Mlist = {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}}\[Transpose];
		dt = 0.1;
		intRes = 8;
		{thetamat,dthetamat} = ForwardDynamicsTrajectory[
			thetalist,dthetalist,taumat,g,Ftipmat,Mlist,Glist,Slist,dt,intRes
		];
		(*Output to plot the joint forces/torques*)
		theta1 = Flatten[thetamat[[;;,1]]];
		theta2 = Flatten[thetamat[[;;,2]]];
		theta3 = Flatten[thetamat[[;;,3]]];
		dtheta1 = Flatten[ dthetamat[[;;,1]]];
		dtheta2 = Flatten[dthetamat[[;;,2]]];
		dtheta3 = Flatten[ dthetamat[[;;,3]]];
		(*PLEASE REPLACE THE SINGLE QUOTES TO DOUBLE TUOTES 
		  BEFORE RUNNING THE COMMAND BELOW*)
		ListLinePlot[
			{theta1,theta2,theta3,dtheta1,dtheta2,dtheta3}, 
			PlotLegends->{'theta1','theta2','theta3',
						  'dtheta1','dtheta2','dtheta3'},
			AxesLabel->{'Time','Joint Angles/Velocities'}, 
			PlotLabel->'Plot of Joint Angles and Joint Velocities', 
			PlotRange->Full
		]"


(* ::Subsection::Closed:: *)
(*CHAPTER 9: TRAJECTORY GENERATION*)


CubicTimeScaling::usage=
"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:
	Inputs:
		Tf = 2;
		t = 0.6;
		s = CubicTimeScaling[Tf,t]
	Output:
		0.216"


QuinticTimeScaling::usage=
"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:
	Inputs:
		Tf = 2;
		t = 0.6;
		s = QuinticTimeScaling[Tf,t]
	Output:
		0.16308"


JointTrajectory::usage=
"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:
	Inputs:
		thetastart = {{1,0,0,1,1,0.2,0,1}}\[Transpose];
		thetaend = {{1.2,0.5,0.6,1.1,2,2,0.9,1}}\[Transpose];
		Tf = 4;
		Np = 6;
		method = 3;
		traj = JointTrajectory[thetastart,thetaend,Tf,Np,method]//N
	Output:
		{{1,0,0,1,1,0.2,0,1},
		 {1.0208,0.052,0.0624,1.0104,1.104,0.3872,0.0936,1},
		 {1.0704,0.176,0.2112,1.0352,1.352,0.8336,0.3168,1}, 
		 {1.1296,0.324,0.3888,1.0648,1.648,1.3664,0.5832,1}, 
		 {1.1792,0.448,0.5376,1.0896,1.896,1.8128,0.8064,1}, 
		 {1.2,0.5,0.6,1.1,2,2,0.9,1}}"


ScrewTrajectory::usage=
"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:
	Inputs:
		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;
		Np=4;
		method=3;
		traj = ScrewTrajectory[Xstart,Xend,Tf,Np,method]
	Output:
		{{{1,0,0,1},{0,1,0,0},{0,0,1,1},{0,0,0,1}}, 
		 {{0.904111,-0.250372,0.346261,0.440958},
		  {0.346261,0.904111,-0.250372,0.528746},
		  {-0.250372,0.346261,0.904111,1.60067},
		  {0,0,0,1}}, 
		 {{0.346261,-0.250372,0.904111,-0.117111},
		  {0.904111,0.346261,-0.250372,0.472742},
		  {-0.250372,0.904111,0.346261,3.274}, 
		  {0,0,0,1}}, 
		 {{0,0,1,0.1},{1,0,0,0},{0,1,0,4.1},{0,0,0,1}}}"


CartesianTrajectory::usage=
"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:
	Inputs:
		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;
		Np = 4;
		method = 5;
		traj = CartesianTrajectory[Xstart,Xend,Tf,Np,method]//N;
	Output:
		{{{1,0,0,1},{0,1,0,0},{0,0,1,1},{0,0,0,1}},
		{{0.936625,-0.214001,0.277376,0.811111},
		 {0.277376,0.936625,-0.214001,0}, 
		 {-0.214001,0.277376,0.936625,1.65062},{0,0,0,1}},
		{{0.277376,-0.214001,0.936625,0.288889},
		 {0.936625,0.277376,-0.214001,0}, 
		 {-0.214001,0.936625,0.277376,3.44938},{0,0,0,1}},
		 {{0,0,1,0.1},{1,0,0,0},{0,1,0,4.1},{0,0,0,1}}}"


(* ::Subsection::Closed:: *)
(*CHAPTER 11: ROBOT CONTROL*)


ComputedTorque::usage=
"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, in the format of a
		   matrix with axes as the columns,
	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:
	Inputs:
		thetalist = {{0.1,0.1,0.1}}\[Transpose];
		dthetalist = {{0.1,0.2,0.3}}\[Transpose];
		eint = {{0.2,0.2,0.2}}\[Transpose];
		g = {{0,0,-9.8}}\[Transpose];
		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 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}];
		G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}];
		G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095,
		                     2.275,2.275,2.275}];
		Glist = {G1,G2,G3};
		Mlist = {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}}\[Transpose];
		thetalistd = {{1,1,1}}\[Transpose];
		dthetalistd = {{2,1.2,2}}\[Transpose];
		ddthetalistd = {{0.1,0.1,0.1}}\[Transpose];
		Kp = 1.3;
		Ki = 1.2;
		Kd = 1.1;
		taulist = ComputedTorque[thetalist,dthetalist,eint,g,Mlist,Glist,
		                         Slist,thetalistd,dthetalistd,ddthetalistd,
								 Kp,Ki,Kd]
	Ouput:
		{{133.005}, {-29.9422}, {-3.03277}}"


SimulateControl::usage=
"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-1 at the home position,
	Glist: Actual spatial inertia matrices Gi of the links,
	Slist: Screw axes Si of the joints in a space frame, in the format of a 
		   matrix with axes as the columns,
	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 controllers 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 (3 Link Robot):
	Inputs:
		thetalist = {{0.1,0.1,0.1}}\[Transpose];
		dthetalist = {{0.1,0.2,0.3}}\[Transpose];
		(*Initialize robot description*)
		g = {{0,0,-9.8}}\[Transpose];
		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 = DiagonalMatrix[{0.010267,0.010267,0.00666,3.7,3.7,3.7}];
		G2 = DiagonalMatrix[{0.22689,0.22689,0.0151074,8.393,8.393,8.393}];
		G3 = DiagonalMatrix[{0.0494433,0.0494433,0.004095,
							 2.275,2.275,2.275}];
		Glist = {G1,G2,G3};
		Mlist = {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}}\[Transpose];
		dt = 0.01;
		(*Create a trajectory to follow*)
		thetaend = {{Pi/2,Pi,1.5*Pi}}\[Transpose];
		Tf = 1;
		No = Tf/dt;
		method = 5;
		traj = JointTrajectory[thetalist,thetaend,Tf,No,method];
		thetamatd = traj;
		dthetamatd = traj;
		ddthetamatd = traj;
		dt = Tf/(No-1.0);
		Do[
			dthetamatd[[i,;;]]=(thetamatd[[i,;;]]-thetamatd[[i-1,;;]])/dt;
			ddthetamatd[[i,;;]]=(dthetamatd[[i,;;]]-dthetamatd[[i-1,;;]])/dt,
			{i,2,Dimensions[traj][[1]]}
		];
		(*Possibly wrong robot description*)
		gtilde = {{0.8,0.2,-8.8}}\[Transpose];
		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 = DiagonalMatrix[{0.1,0.1,0.1,4,4,4}];
		Ghat2 = DiagonalMatrix[{0.3,0.3,0.1,9,9,9}];
		Ghat3 = DiagonalMatrix[{0.1,0.1,0.1,3,3,3}];
		Gtildelist = {Ghat1,Ghat2,Ghat3};
		Mtildelist = {Mhat01,Mhat12,Mhat23,Mhat34};
		Ftipmat = ConstantArray[1,{Dimensions[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];"


(* ::Section::Closed:: *)
(*CODE*)


Begin["`Private`"];


(* ::Subsection::Closed:: *)
(*BASIC HELPER FUNCTIONS*)


NearZero[s_]:=Abs[s]<10^-6


(* ::Subsection::Closed:: *)
(*CHAPTER 3: RIGID-BODY MOTION*)


RotInv[R_]:=R\[Transpose]


VecToso3[omg_]:={{0,-omg[[3,1]],omg[[2,1]]},{omg[[3,1]],0,-omg[[1,1]]},
				 {-omg[[2,1]],omg[[1,1]],0}}


so3ToVec[so3mat_]:={{so3mat[[3,2]],so3mat[[1,3]],so3mat[[2,1]]}}\[Transpose]


AxisAng3[expc3_]:={expc3/Norm[expc3],Norm[expc3]}


MatrixExp3[so3mat_]:=Module[
	{omgtheta,omghat,theta,omgmat},
	omgtheta=so3ToVec[so3mat];
	If[
		NearZero[Norm[omgtheta]],
		Return[IdentityMatrix[3]],
		{omghat, theta}=AxisAng3[omgtheta];
		omgmat=so3mat/theta;
		Return[IdentityMatrix[3]+Sin[theta]*omgmat+
			   (1-Cos[theta])*omgmat.omgmat]
	]
]


MatrixLog3[R_]:=Module[
	{acosinput,theta,omgmat},
	acosinput=(Tr[R]-1)/2;
	Which[
		acosinput>=1,
		Return[ConstantArray[0,{3,3}]],
		acosinput<=-1,
		Which[
			Not[NearZero[R[[3,3]]+1]],
			Return[VecToso3[
				1/Sqrt[2(1+R[[3,3]])]*
				{{R[[1,3]],R[[2,3]],1+R[[3,3]]}}\[Transpose]*Pi
			]],
			Not[NearZero[R[[2,2]]+1]],
			Return[VecToso3[
				1/Sqrt[2(1+R[[2,2]])]*
				{{R[[1,2]],1+R[[2,2]],R[[3,2]]}}\[Transpose]*Pi
			]],
			True,
			Return[VecToso3[
				1/Sqrt[2(1+R[[1,1]])]*
				{{1+R[[1,1]],R[[2,1]],R[[3,1]]}}\[Transpose]*Pi
			]]
		],
		True,
		theta=ArcCos[acosinput];
		omgmat=(R-R\[Transpose])/(2*Sin[theta]);
		Return[theta*omgmat]					
	]
]


DistanceToSO3[mat_]:=If[
	Det[mat]>0, 
	Return[Norm[mat\[Transpose].mat-IdentityMatrix[3],"Frobenius"]], 
	Return[10^9]
]


TestIfSO3[mat_]:=Abs[DistanceToSO3[mat]]<10^-3


ProjectToSO3[mat_]:=Module[
	{R,u,w,v}, 
	{u,w,v}=SingularValueDecomposition[mat]; 
	R=u.v\[Transpose];
	(*In the case below, the result may be far from mat.*)
	If[
		Det[R] < 0, 
		Return[ArrayFlatten[{{R[[;;,1;;2]],{-R[[;;,3]]}\[Transpose]}}]],
		Return[R]
	]
]


RpToTrans[R_,p_]:=ArrayFlatten[{{R,p},{0,1}}]


TransToRp[T_]:={T[[1;;3,1;;3]],{T[[1;;3,4]]}\[Transpose]}


TransInv[T_]:=ArrayFlatten[
	{{T[[1;;3,1;;3]]\[Transpose],
	  -T[[1;;3,1;;3]]\[Transpose].{T[[1;;3,4]]}\[Transpose]},
	 {0,1}}
]


VecTose3[V_]:=ArrayFlatten[
	{{VecToso3[{V[[1;;3,1]]}\[Transpose]],{V[[4;;6,1]]}\[Transpose]},{0,0}}
]


se3ToVec[se3mat_]:={{se3mat[[3,2]],se3mat[[1,3]],se3mat[[2,1]],
                     se3mat[[1,4]],se3mat[[2,4]],se3mat[[3,4]]}}\[Transpose]


Adjoint[T_]:=Module[
	{R,p},
	{R,p}=TransToRp[T];
	Return[ArrayFlatten[{{R,0},{VecToso3[p].R,R}}]]
]


ScrewToAxis[q_,s_,h_]:=ArrayFlatten[{{s},{{Cross[Flatten[q],
										   Flatten[s]]}\[Transpose]+h*s}}]


AxisAng6[expc6_]:=Module[
	{theta},
	theta=Norm[{expc6[[1;;3,1]]}\[Transpose]];
	If[NearZero[theta],theta=Norm[{expc6[[4;;6,1]]}\[Transpose]]];
	Return[{expc6/theta,theta}]
]


MatrixExp6[se3mat_]:=Module[
	{omgtheta,omghat,theta,omgmat,G},
	omgtheta=so3ToVec[se3mat[[1;;3,1;;3]]];
	If[
		NearZero[Norm[omgtheta]],
		Return[ArrayFlatten[
			{{IdentityMatrix[3],{se3mat[[1;;3,4]]}\[Transpose]},{0,1}}
		]],
		{omghat,theta} = AxisAng3[omgtheta];
		omgmat=se3mat[[1;;3,1;;3]]/theta;
		Return[ArrayFlatten[
			{{MatrixExp3[se3mat[[1;;3,1;;3]]],
			  (IdentityMatrix[3]*theta+(1-Cos[theta])*omgmat+
			   (theta-Sin[theta])*omgmat.omgmat).
			  ({se3mat[[1;;3,4]]}\[Transpose]/theta)},
			 {0,1}}
		]]
	]
]


MatrixLog6[T_]:=Module[
	{R,p,acosinput,theta,omgmat},
	{R,p}=TransToRp[T];
	omgmat=MatrixLog3[R];
	If[
		omgmat==ConstantArray[0,{3,3}],
		ArrayFlatten[
			{{ConstantArray[0,{3,3}],{T[[1;;3,4]]}\[Transpose]},{0,0}}
		],
		theta=ArcCos[(Tr[R]-1)/2];			
		Return[ArrayFlatten[{
			{omgmat,(IdentityMatrix[3]-omgmat/2+
					 (1/theta-Cot[theta/2]/2)*omgmat.omgmat/theta).p},
			{0,0}
		}]]	
	]
]


DistanceToSE3[mat_]:=Module[
	{R,p}, 
	{R,p}=TransToRp[mat]; 
	If[
		Det[R] > 0, 
		Return[Norm[
			Join[Join[R\[Transpose].R,{{0,0,0}}\[Transpose],2],{mat[[4,;;]]}]-
			IdentityMatrix[4],
			"Frobenius"
		]], 
		Return[10^9]
	]
]


TestIfSE3[mat_]:=Abs[DistanceToSE3[mat]]<10^-3


ProjectToSE3[mat_]:=RpToTrans[ProjectToSO3[mat[[1;;3,1;;3]]],
							  {mat[[1;;3,4]]}\[Transpose]]


(* ::Subsection::Closed:: *)
(*CHAPTER 4: FORWARD KINEMATICS*)


FKinBody[M_,Blist_,thetalist_]:=Module[
	{n,T,i},
	T=M;
	Do[
		T=T.MatrixExp6[
			VecTose3[{Blist[[;;,i]]}\[Transpose]*thetalist[[i,1]]]
		],
		{i,Length[thetalist]}
	];
	Return[T]
]


FKinSpace[M_,Slist_,thetalist_]:=Module[
	{n,T},
	T=M;
	Do[
		T=MatrixExp6[
			VecTose3[{Slist[[;;,i]]}\[Transpose]*thetalist[[i,1]]]
		].T,
		{i,Length[thetalist],1,-1}
	];
	Return[T]
]


(* ::Subsection::Closed:: *)
(*CHAPTER 5: VELOCITY KINEMATICS AND STATICS*)


JacobianBody[Blist_,thetalist_]:=Module[
	{Jb=Blist,T=IdentityMatrix[4]},
	Do[
		T=T.MatrixExp6[VecTose3[{Blist[[;;,i+1]]}\[Transpose]*
								-thetalist[[i+1,1]]]];
		Jb[[;;,i]]=Flatten[Adjoint[T].{Blist[[;;,i]]}\[Transpose]],
		{i,Length[thetalist]-1,1,-1}
	];
	Return[Jb]
]


JacobianSpace[Slist_,thetalist_]:=Module[
	{Js=Slist,T=IdentityMatrix[4]},
	Do[
		T=T.MatrixExp6[VecTose3[{Slist[[;;,i-1]]}\[Transpose]*
								thetalist[[i-1,1]]]];
		Js[[;;,i]]=Flatten[Adjoint[T].{Slist[[;;,i]]}\[Transpose]],
		{i,2,Length[thetalist]}
	];
	Return[Js]
]


(* ::Subsection::Closed:: *)
(*CHAPTER 6: INVERSE KINEMATICS*)


IKinBody[Blist_,M_,T_,thetalist0_,eomg_,ev_]:=Module[
	{Vb,thetalist=thetalist0,err,i=0,maxiterations=20},
	Vb = se3ToVec[MatrixLog6[TransInv[FKinBody[M,Blist,thetalist]].T]];
	err=Norm[Vb[[1;;3,1]]]>eomg || Norm[Vb[[4;;6,1]]]>ev;
	While[
		err&&i<maxiterations,
		thetalist=thetalist+PseudoInverse[JacobianBody[Blist,thetalist]].Vb;
		i++;
		Vb=se3ToVec[MatrixLog6[TransInv[FKinBody[M,Blist,thetalist]].T]];
		err=Norm[Vb[[1;;3,1]]]>eomg||Norm[Vb[[4;;6,1]]]>ev
	];
	Return[{thetalist, !err}]
]


IKinSpace[Slist_,M_,T_,thetalist0_,eomg_,ev_]:=Module[
	{Tsb,Vs,thetalist=thetalist0,err,i=0,maxiterations=20},
	Tsb=FKinSpace[M,Slist,thetalist];
	Vs = Adjoint[Tsb].se3ToVec[MatrixLog6[TransInv[Tsb].T]];
	err=Norm[Vs[[1;;3,1]]]>eomg||Norm[Vs[[4;;6,1]]]>ev;
	While[
		err&&i<maxiterations,
		thetalist=thetalist+PseudoInverse[JacobianSpace[Slist,thetalist]].Vs;
		i++;
		Tsb=FKinSpace[M,Slist,thetalist];
		Vs = Adjoint[Tsb].se3ToVec[MatrixLog6[TransInv[Tsb].T]];
		err=Norm[Vs[[1;;3,1]]]>eomg || Norm[Vs[[4;;6,1]]]>ev
	];
	Return[{thetalist, !err}]
]


(* ::Subsection::Closed:: *)
(*CHAPTER 8: DYNAMICS OF OPEN CHAINS*)


ad[V_]:=Module[
	{w},
	w=VecToso3[V[[1;;3]]];
	Return[ArrayFlatten[{{w,0},{VecToso3[V[[4;;6]]],w}}]]
]


InverseDynamics[thetalist_,dthetalist_,ddthetalist_,g_,Ftip_,Mlist_,Glist_,Slist_]:=Module[
	{n ,Ai,AdTi,Ti,Vi,Vdi,Fi=Ftip,taulist,Mi=IdentityMatrix[4]},
	n=Length[thetalist];
	Ai=ConstantArray[0,{6,n}];
	AdTi=ConstantArray[Null,{1,n+1}];
	Vi=ConstantArray[0,{6,n+1}];
	Vdi=ConstantArray[0,{6,n+1}];
	Vdi[[4;;6,1]]=Flatten[-g];
	AdTi[[1,n+1]]=Adjoint[TransInv[Mlist[[n+1]]]];
	taulist=ConstantArray[0,{n,1}];
	Do[
		Mi=Mi.Mlist[[i]];
		Ai[[;;,i]]=Adjoint[TransInv[Mi]].Slist[[;;,i]];
		AdTi[[1,i]]=Adjoint[MatrixExp6[
			VecTose3[{Ai[[;;,i]]}\[Transpose]*-thetalist[[i,1]]]
		].TransInv[Mlist[[i]]]];
		Vi[[;;,i+1]]=AdTi[[1,i]].Vi[[;;,i]]+
					 {Ai[[;;,i]]}\[Transpose]*dthetalist[[i,1]];
		Vdi[[;;,i+1]]=AdTi[[1,i]].Vdi[[;;,i]]+
					  {Ai[[;;,i]]}\[Transpose]*ddthetalist[[i,1]]+
					  ad[Vi[[;;,i+1]]].Ai[[;;,i]]*dthetalist[[i,1]],
		{i,1,n}
	];
	Do[
		Fi=AdTi[[1,i+1]]\[Transpose].Fi+Glist[[i]].Vdi[[;;,i+1]]-
		   ad[Vi[[;;,i+1]]]\[Transpose].(Glist[[i]].Vi[[;;,i+1]]);
		taulist[[i,1]]=(Fi\[Transpose].Ai[[;;,i]])[[1]],
		{i,n,1,-1}
	];
	Return[taulist]
]


MassMatrix[thetalist_,Mlist_,Glist_,Slist_]:=Module[
	{n,dthetalist,ddthetalist,g,Ftip,taulist,M},
	n=Length[thetalist];
	M=ConstantArray[0,{n,n}];
	Do[
		ddthetalist=ConstantArray[0,{n,1}];
		ddthetalist[[i,1]]=1;
		M[[;;,i]]=Flatten[InverseDynamics[thetalist,ConstantArray[0,{6,n}],
										  ddthetalist,{{0,0,0}}\[Transpose],
										  {{0,0,0,0,0,0}}\[Transpose],Mlist,
										  Glist,Slist]],
		{i,n}
	];
	Return[M]
]


VelQuadraticForces[thetalist_,dthetalist_,Mlist_,Glist_,Slist_]:=
	InverseDynamics[thetalist,dthetalist,
					ConstantArray[0,{Length[thetalist],1}],
					{{0,0,0}}\[Transpose],{{0,0,0,0,0,0}}\[Transpose],Mlist,
					Glist,Slist]


GravityForces[thetalist_,g_,Mlist_,Glist_,Slist_]:=Module[
	{n=Length[thetalist]},
	Return[InverseDynamics[
			thetalist,ConstantArray[0,{n,1}],ConstantArray[0,{n,1}],g,
			{{0,0,0,0,0,0}}\[Transpose],Mlist,Glist,Slist
	]]
]


EndEffectorForces[thetalist_,Ftip_,Mlist_,Glist_,Slist_]:=Module[
	{n=Length[thetalist]},
	Return[InverseDynamics[
		thetalist,ConstantArray[0,{n,1}],ConstantArray[0,{n,1}],
		{{0,0,0}}\[Transpose],Ftip,Mlist,Glist,Slist
	]]
]


ForwardDynamics[thetalist_,dthetalist_,taulist_,g_,Ftip_,Mlist_,Glist_,Slist_]:=
	Inverse[MassMatrix[thetalist,Mlist,Glist,Slist]].
	(taulist-VelQuadraticForces[thetalist,dthetalist,Mlist,Glist,Slist]-
	 GravityForces[thetalist,g,Mlist,Glist,Slist]-
	 EndEffectorForces[thetalist,Ftip,Mlist,Glist,Slist])


EulerStep[thetalist_,dthetalist_,ddthetalist_,dt_]:=
	{thetalist+dt*dthetalist,dthetalist+dt*ddthetalist}


InverseDynamicsTrajectory[thetamat_,dthetamat_,ddthetamat_,g_,Ftipmat_,Mlist_,Glist_,Slist_]:=Module[
	{thetamatt=thetamat\[Transpose],dthetamatt=dthetamat\[Transpose],
	 ddthetamatt=ddthetamat\[Transpose],Ftipmatt=Ftipmat\[Transpose],taumat},
	taumat=thetamatt;
	Do[
		taumat[[;;,i]]=InverseDynamics[{thetamatt[[;;,i]]}\[Transpose],
									   {dthetamatt[[;;,i]]}\[Transpose],
									   {ddthetamatt[[;;,i]]}\[Transpose],g,
									   Ftipmatt[[;;,i]],Mlist,Glist,Slist],
		{i,Dimensions[thetamatt][[2]]}
	];
	taumat=taumat\[Transpose];
	Return[taumat]
]


ForwardDynamicsTrajectory[thetalist_,dthetalist_,taumat_,g_,Ftipmat_,Mlist_,Glist_,Slist_,dt_,intRes_]:=Module[
	{taumatt=taumat\[Transpose],Ftipmatt=Ftipmat\[Transpose],thetamat,
	 dthetamat,ddthetanext,thetanext=thetalist,dthetanext=dthetalist},
	thetamat=taumatt;
	dthetamat=taumatt;
	thetamat[[;;,1]]=thetanext;
	dthetamat[[;;,1]]=dthetanext;
	Do[
		Do[
			ddthetanext=ForwardDynamics[thetanext,dthetanext,taumatt[[;;,i]],
										g,Ftipmatt[[;;,i]],Mlist,Glist,
										Slist];
			{thetanext,dthetanext}=EulerStep[thetanext,dthetanext,
											 ddthetanext,dt/intRes],
			intRes
		];
		thetamat[[;;,i+1]]=thetanext;
		dthetamat[[;;,i+1]]=dthetanext,
		{i,Dimensions[taumatt][[2]]-1}
	];
	thetamat=thetamat\[Transpose];
	dthetamat=dthetamat\[Transpose];
	Return[{thetamat,dthetamat}]
]


(* ::Subsection::Closed:: *)
(*CHAPTER 9: TRAJECTORY GENERATION*)


CubicTimeScaling[Tf_,t_]:=3*(t/Tf)^2-2*(t/Tf)^3


QuinticTimeScaling[Tf_,t_]:=10*(t/Tf)^3-15*(t/Tf)^4+6*(t/Tf)^5


JointTrajectory[thetastart_,thetaend_,Tf_,Num_,method_]:=Module[
	{Numb=Round[Num],timegap,traj,s},
	timegap=Tf/(Numb-1);
	traj= ConstantArray[0,{Length[thetastart],Numb}];
	Do[
		If[method == 3,s=CubicTimeScaling[Tf,(i-1)*timegap],
					   s=QuinticTimeScaling[Tf,(i-1)*timegap]];
		traj[[;;,i]]=Flatten[thetastart +s*(thetaend-thetastart)],
		{i,Numb}
	];
	traj=traj\[Transpose];
	Return[traj]
]


ScrewTrajectory[Xstart_,Xend_,Tf_,N_,method_]:=Module[
	{timegap=Tf/(N-1),traj=ConstantArray[Null,N],s},
	Do[
		If[method==3,s=CubicTimeScaling[Tf,(i-1)*timegap],
		   s=QuinticTimeScaling[Tf,(i-1)*timegap]];
		traj[[i]]=Xstart.MatrixExp6[MatrixLog6[TransInv[Xstart].Xend]*s],
		{i,N}
	];
	Return[traj]
]


CartesianTrajectory[Xstart_,Xend_,Tf_,N_,method_]:=Module[
	{timegap=Tf/(N-1),traj=ConstantArray[Null,N],s,Rstart,pstart,Rend,pend},
	{Rstart,pstart}=TransToRp[Xstart];{Rend,pend}=TransToRp[Xend];
	Do[
		If[
			method == 3,
			s=CubicTimeScaling[Tf,(i-1)*timegap],
			s=QuinticTimeScaling[Tf,(i-1)*timegap]
		];
		traj[[i]]=ArrayFlatten[
			{{Rstart.MatrixExp3[MatrixLog3[Rstart\[Transpose].Rend]*s],
			  pstart+s*(pend-pstart)},
			 {0,1}}
		],
		{i,N}
	];
	Return[traj]
]


(* ::Subsection::Closed:: *)
(*CHAPTER 11: ROBOT CONTROL*)


ComputedTorque[thetalist_,dthetalist_,eint_,g_,Mlist_,Glist_,Slist_,thetalistd_,dthetalistd_,ddthetalistd_,Kp_,Ki_,Kd_]:=Module[
	{e=thetalistd-thetalist},
	Return[
		MassMatrix[thetalist,Mlist,Glist,Slist].
		(Kp*e+Ki*(eint+e)+Kd*(dthetalistd-dthetalist))+
		InverseDynamics[thetalist,dthetalist,ddthetalistd,g,
						{{0,0,0,0,0,0}}\[Transpose],Mlist,Glist,Slist]
	]
]


SimulateControl[thetalist_,dthetalist_,g_,Ftipmat_,Mlist_,Glist_,Slist_,thetamatd_,dthetamatd_,ddthetamatd_,gtilde_,Mtildelist_,Gtildelist_,Kp_,Ki_,Kd_,dt_,intRes_]:=Module[
	{Ftipmatt=Ftipmat\[Transpose],thetamatdt=thetamatd\[Transpose],
	 dthetamatdt=dthetamatd\[Transpose],ddthetamatdt=ddthetamatd\[Transpose],
	 taumat,thetamat,n,thetacurrent,dthetacurrent,eint,taulist,ddthetalist,
	 links, plotAngles, labels,style,col},
	n=Dimensions[thetamatdt][[2]];
	thetamat=thetamatdt;
	taumat=thetamatdt;
	thetacurrent=thetalist;
	dthetacurrent=dthetalist;
	eint=ConstantArray[0,{Dimensions[thetamatdt][[1]],1}];
	Do[
		taulist=
			ComputedTorque[thetacurrent,dthetacurrent,eint,gtilde,Mtildelist,
						   Gtildelist,Slist,{thetamatdt[[;;,i]]}\[Transpose],
						   {dthetamatdt[[;;,i]]}\[Transpose],
						   {ddthetamatdt[[;;,i]]}\[Transpose],Kp,Ki,Kd];
		Do[
			ddthetalist=ForwardDynamics[thetacurrent,dthetacurrent,taulist,g,
										{Ftipmatt[[;;,i]]}\[Transpose],Mlist,
										Glist,Slist];
			{thetacurrent,dthetacurrent}=
				EulerStep[thetacurrent,dthetacurrent,ddthetalist,
						  (dt/intRes)],		
			{j,intRes}
		];
		taumat[[;;,i]]=taulist;
		thetamat[[;;,i]]=thetacurrent;
		eint=eint+(dt*(thetamatdt[[;;,i]]-thetacurrent)),
		{i,n}
	];
	(*Output plotting*)
	links=Dimensions[thetamat][[1]];
	plotAngles ={};
	labels = {};
	style={};
	Do[AppendTo[plotAngles, Flatten[thetamat[[i,;;]]]];
	AppendTo[plotAngles, Flatten[thetamatdt[[i,;;]]]];
	AppendTo[labels, StringJoin["ActualTheta",ToString[i]]];
	AppendTo[labels, StringJoin["DesiredTheta",ToString[i]]];
	col = RandomColor[];
	AppendTo[style, col];
	AppendTo[style, {col,Dashed}],{i,1,links}];
	Print[ListLinePlot[plotAngles,PlotLegends->labels,PlotStyle->style,
					   AxesLabel->{"Time","Joint Angles"},
					   PlotLabel->"Plot of Actual and Desired Joint Angles",
					   PlotRange->Full]];
	taumat=taumat\[Transpose];
	thetamat=thetamat\[Transpose];
	Return[{taumat,thetamat}]
]


(* ::Section::Closed:: *)
(*END*)


End[];


Protect["NearZero"];
Protect["RotInv","VecToso3","so3ToVec","AxisAng3","MatrixExp3","MatrixLog3",
		"DistanceToSO3","TestIfSO3","ProjectToSO3","RpToTrans","TransToRp",
		"TransInv","VecTose3","se3ToVec","Adjoint","ScrewToAxis","AxisAng6",
		"MatrixExp6","MatrixLog6","DistanceToSE3","TestIfSE3",
		"ProjectToSE3"];
Protect["FKinBody","FKinSpace"];
Protect["JacobianBody","JacobianSpace"];
Protect["IKinBody","IKinSpace"];
Protect["ad","InverseDynamics","MassMatrix","VelQuadraticForces",
		"GravityForces","EndEffectorForces","ForwardDynamics","EulerStep",
		"InverseDynamicsTrajectory","ForwardDynamicsTrajectory"];
Protect["CubicTimeScaling","QuinticTimeScaling","JointTrajectory",
		"ScrewTrajectory","CartesianTrajectory"];
Protect["ComputedTorque","SimulateControl"];


EndPackage[];