14 |
15 |
--------------------------------------------------------------------------------
/mr/ProjectToSO3.m:
--------------------------------------------------------------------------------
1 | function R = ProjectToSO3(mat)
2 | % *** CHAPTER 3: RIGID-BODY MOTIONS ***
3 | % Takes mat: A matrix near SO(3) to project to SO(3).
4 | % Returns R representing the closest rotation matrix that is in SO(3).
5 | % This function uses singular-value decomposition (see
6 | % http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review)
7 | % and is only appropriate for matrices close to SO(3).
8 | % Example Inputs:
9 | %
10 | % clear; clc;
11 | % mat = [ 0.675, 0.150, 0.720;
12 | % 0.370, 0.771, -0.511;
13 | % -0.630, 0.619, 0.472];
14 | % R = ProjectToSO3(mat)
15 | %
16 | % Output:
17 | % R =
18 | % 0.6790 0.1489 0.7189
19 | % 0.3732 0.7732 -0.5127
20 | % -0.6322 0.6164 0.4694
21 |
22 | [U, S, V] = svd(mat);
23 | R = U * V';
24 | if det(R) < 0
25 | % In this case the result may be far from mat.
26 | R = [R(:, 1: 2); -R(:, 3)];
27 | end
28 | end
--------------------------------------------------------------------------------
/contat_lib/Help/Box_to_Box_Contact.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
11 |
12 | 
14 |
15 |
--------------------------------------------------------------------------------
/mr/DistanceToSE3.m:
--------------------------------------------------------------------------------
1 | function d = DistanceToSE3(mat)
2 | % *** CHAPTER 3: RIGID-BODY MOTIONS ***
3 | % Takes mat: A 4x4 matrix.
4 | % Returns the Frobenius norm to describe the distance of mat from the SE(3)
5 | % manifold.
6 | % Compute the determinant of matR, the top 3x3 submatrix of mat. If
7 | % det(matR) <= 0, return a large number. If det(matR) > 0, replace the top
8 | % 3x3 submatrix of mat with matR' * matR, and set the first three entries
9 | % of the fourth column of mat to zero. Then return norm(mat - I).
10 | % Example Inputs:
11 | %
12 | % clear; clc;
13 | % mat = [1.0, 0.0, 0.0, 1.2;
14 | % 0.0, 0.1, -0.95, 1.5;
15 | % 0.0, 1.0, 0.1, -0.9;
16 | % 0.0, 0.0, 0.1, 0.98];
17 | % d = DistanceToSE3(mat)
18 | %
19 | % Output:
20 | % d =
21 | % 0.1349
22 |
23 | [R, p] = TransToRp(mat);
24 | if det(R) > 0
25 | d = norm([R' * R, [0; 0; 0]; mat(4, :)] - eye(4), 'fro');
26 | else
27 | d = 1e+9;
28 | end
29 | end
--------------------------------------------------------------------------------
/mr/ProjectToSE3.m:
--------------------------------------------------------------------------------
1 | function T = ProjectToSE3(mat)
2 | % *** CHAPTER 3: RIGID-BODY MOTIONS ***
3 | % Takes mat: A matrix near SE(3) to project to SE(3).
4 | % Returns T representing the closest rotation matrix that is in SE(3).
5 | % This function uses singular-value decomposition (see
6 | % http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review)
7 | % and is only appropriate for matrices close to SE(3).
8 | % Example Inputs:
9 | %
10 | % clear; clc;
11 | % mat = [ 0.675, 0.150, 0.720, 1.2;
12 | % 0.370, 0.771, -0.511, 5.4;
13 | % -0.630, 0.619, 0.472, 3.6;
14 | % 0.003, 0.002, 0.010, 0.9];
15 | % T = ProjectToSE3(mat)
16 | %
17 | % Output:
18 | % T =
19 | % 0.6790 0.1489 0.7189 1.2000
20 | % 0.3732 0.7732 -0.5127 5.4000
21 | % -0.6322 0.6164 0.4694 3.6000
22 | % 0 0 0 1.0000
23 |
24 | T = RpToTrans(ProjectToSO3(mat(1: 3, 1: 3)), mat(1: 3, 4));
25 | end
--------------------------------------------------------------------------------
/contat_lib/Help/Contact_Forces_Library_Use.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
11 |
12 | 
14 |
15 |
--------------------------------------------------------------------------------
/derive_FK_Jacobian.m:
--------------------------------------------------------------------------------
1 | syms q1 q2 q3 real
2 | init_quad_whole_body_ctrl;
3 | % sym modern robotics fk
4 |
5 | thetalist =[q1; q2; q3];
6 |
7 | fk_space = simplify(FKinSpace_sym(quad_param.M3, quad_param.Slist_link2, thetalist));
8 | Js_space = JacobianSpace_sym(quad_param.Slist_link2, thetalist);
9 |
10 | matlabFunction(fk_space,...
11 | 'file','autoGen_fk_space.m',...
12 | 'vars',{q1,q2,q3},...
13 | 'outputs',{'fk_space'});
14 | matlabFunction(Js_space,...
15 | 'file','autoGen_J_space.m',...
16 | 'vars',{q1,q2,q3},...
17 | 'outputs',{'Js'});
18 |
19 | fk_body = simplify(FKinBody_sym(quad_param.M3, quad_param.Blist_link2, thetalist));
20 | Js_body = JacobianBody_sym(quad_param.Blist_link2, thetalist);
21 |
22 | matlabFunction(fk_body,...
23 | 'file','autoGen_fk_body.m',...
24 | 'vars',{q1,q2,q3},...
25 | 'outputs',{'fk_body'});
26 | matlabFunction(Js_body,...
27 | 'file','autoGen_J_body.m',...
28 | 'vars',{q1,q2,q3},...
29 | 'outputs',{'Jb'});
--------------------------------------------------------------------------------
/contat_lib/Help/Box_to_Box_Contact_Forces_BoxB_Corners.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
11 |
12 | 
14 |
15 |
--------------------------------------------------------------------------------
/contat_lib/Help/Box_to_Box_Contact_Forces_BoxF_Corners.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
11 |
12 | 
14 |
15 |
--------------------------------------------------------------------------------
/mr/MatrixLog6.m:
--------------------------------------------------------------------------------
1 | function expmat = MatrixLog6(T)
2 | % *** CHAPTER 3: RIGID-BODY MOTIONS ***
3 | % Takes a transformation matrix T in SE(3).
4 | % Returns the corresponding se(3) representation of exponential
5 | % coordinates.
6 | % Example Input:
7 | %
8 | % clear; clc;
9 | % T = [[1, 0, 0, 0]; [0, 0, -1, 0]; [0, 1, 0, 3]; [0, 0, 0, 1]];
10 | % expmat = MatrixLog6(T)
11 | %
12 | % Output:
13 | % expc6 =
14 | % 0 0 0 0
15 | % 0 0 -1.5708 2.3562
16 | % 0 1.5708 0 2.3562
17 | % 0 0 0 0
18 |
19 | [R, p] = TransToRp(T);
20 | omgmat = MatrixLog3(R);
21 | if isequal(omgmat, zeros(3))
22 | expmat = [zeros(3), T(1: 3, 4); 0, 0, 0, 0];
23 | else
24 | theta = acos((trace(R) - 1) / 2);
25 | expmat = [omgmat, (eye(3) - omgmat / 2 ...
26 | + (1 / theta - cot(theta / 2) / 2) ...
27 | * omgmat * omgmat / theta) * p;
28 | 0, 0, 0, 0];
29 | end
30 | end
--------------------------------------------------------------------------------
/mr/FKinBody.m:
--------------------------------------------------------------------------------
1 | function T = FKinBody(M, Blist, thetalist)
2 | % *** CHAPTER 4: FORWARD KINEMATICS ***
3 | % Takes M: the home configuration (position and orientation) of the
4 | % end-effector,
5 | % Blist: The joint screw axes in the end-effector frame when the
6 | % manipulator is at the home position,
7 | % thetalist: A list of joint coordinates.
8 | % Returns T in SE(3) representing the end-effector frame when the joints
9 | % are at the specified coordinates (i.t.o Body Frame).
10 | % Example Inputs:
11 | %
12 | % clear; clc;
13 | % M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]];
14 | % Blist = [[0; 0; -1; 2; 0; 0], [0; 0; 0; 0; 1; 0], [0; 0; 1; 0; 0; 0.1]];
15 | % thetalist = [pi / 2; 3; pi];
16 | % T = FKinBody(M, Blist, thetalist)
17 | %
18 | % Output:
19 | % T =
20 | % -0.0000 1.0000 0 -5.0000
21 | % 1.0000 0.0000 0 4.0000
22 | % 0 0 -1.0000 1.6858
23 | % 0 0 0 1.0000
24 |
25 | T = M;
26 | for i = 1: size(thetalist)
27 | T = T * MatrixExp6(VecTose3(Blist(:, i) * thetalist(i)));
28 | end
29 | end
--------------------------------------------------------------------------------
/mr/FKinBody_sym.m:
--------------------------------------------------------------------------------
1 | function T = FKinBody_sym(M, Blist, thetalist)
2 | % *** CHAPTER 4: FORWARD KINEMATICS ***
3 | % Takes M: the home configuration (position and orientation) of the
4 | % end-effector,
5 | % Blist: The joint screw axes in the end-effector frame when the
6 | % manipulator is at the home position,
7 | % thetalist: A list of joint coordinates.
8 | % Returns T in SE(3) representing the end-effector frame when the joints
9 | % are at the specified coordinates (i.t.o Body Frame).
10 | % Example Inputs:
11 | %
12 | % clear; clc;
13 | % M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]];
14 | % Blist = [[0; 0; -1; 2; 0; 0], [0; 0; 0; 0; 1; 0], [0; 0; 1; 0; 0; 0.1]];
15 | % thetalist = [pi / 2; 3; pi];
16 | % T = FKinBody(M, Blist, thetalist)
17 | %
18 | % Output:
19 | % T =
20 | % -0.0000 1.0000 0 -5.0000
21 | % 1.0000 0.0000 0 4.0000
22 | % 0 0 -1.0000 1.6858
23 | % 0 0 0 1.0000
24 |
25 | T = M;
26 | for i = 1: size(thetalist)
27 | T = T * simplify(MatrixExp6_sym(VecTose3(Blist(:, i)),thetalist(i)));
28 | end
29 | end
--------------------------------------------------------------------------------
/mr/MatrixLog3.m:
--------------------------------------------------------------------------------
1 | function so3mat = MatrixLog3(R)
2 | % *** CHAPTER 3: RIGID-BODY MOTIONS ***
3 | % Takes R (rotation matrix).
4 | % Returns the corresponding so(3) representation of exponential
5 | % coordinates.
6 | % Example Input:
7 | %
8 | % clear; clc;
9 | % R = [[0, 0, 1]; [1, 0, 0]; [0, 1, 0]];
10 | % so3mat = MatrixLog3(R)
11 | %
12 | % Output:
13 | % angvmat =
14 | % 0 -1.2092 1.2092
15 | % 1.2092 0 -1.2092
16 | % -1.2092 1.2092 0
17 |
18 | acosinput = (trace(R) - 1) / 2;
19 | if acosinput >= 1
20 | so3mat = zeros(3);
21 | elseif acosinput <= -1
22 | if ~NearZero(1 + R(3, 3))
23 | omg = (1 / sqrt(2 * (1 + R(3, 3)))) ...
24 | * [R(1, 3); R(2, 3); 1 + R(3, 3)];
25 | elseif ~NearZero(1 + R(2, 2))
26 | omg = (1 / sqrt(2 * (1 + R(2, 2)))) ...
27 | * [R(1, 2); 1 + R(2, 2); R(3, 2)];
28 | else
29 | omg = (1 / sqrt(2 * (1 + R(1, 1)))) ...
30 | * [1 + R(1, 1); R(2, 1); R(3, 1)];
31 | end
32 | so3mat = VecToso3(pi * omg);
33 | else
34 | theta = acos(acosinput);
35 | so3mat = theta * (1 / (2 * sin(theta))) * (R - R');
36 | end
37 | end
--------------------------------------------------------------------------------
/mr/FKinSpace.m:
--------------------------------------------------------------------------------
1 | function T = FKinSpace(M, Slist, thetalist)
2 | % *** CHAPTER 4: FORWARD KINEMATICS ***
3 | % Takes M: the home configuration (position and orientation) of the
4 | % end-effector,
5 | % Slist: The joint screw axes in the space frame when the manipulator
6 | % is at the home position,
7 | % thetalist: A list of joint coordinates.
8 | % Returns T in SE(3) representing the end-effector frame, when the joints
9 | % are at the specified coordinates (i.t.o Space Frame).
10 | % Example Inputs:
11 | %
12 | % clear; clc;
13 | % M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]];
14 | % Slist = [[0; 0; 1; 4; 0; 0], ...
15 | % [0; 0; 0; 0; 1; 0], ...
16 | % [0; 0; -1; -6; 0; -0.1]];
17 | % thetalist =[pi / 2; 3; pi];
18 | % T = FKinSpace(M, Slist, thetalist)
19 | %
20 | % Output:
21 | % T =
22 | % -0.0000 1.0000 0 -5.0000
23 | % 1.0000 0.0000 0 4.0000
24 | % 0 0 -1.0000 1.6858
25 | % 0 0 0 1.0000
26 |
27 | T = M;
28 | for i = size(thetalist): -1: 1
29 | T = MatrixExp6(VecTose3(Slist(:, i) * thetalist(i))) * T;
30 | end
31 | end
--------------------------------------------------------------------------------
/mr/FKinSpace_sym.m:
--------------------------------------------------------------------------------
1 | function T = FKinSpace_sym(M, Slist, thetalist)
2 | % *** CHAPTER 4: FORWARD KINEMATICS ***
3 | % Takes M: the home configuration (position and orientation) of the
4 | % end-effector,
5 | % Slist: The joint screw axes in the space frame when the manipulator
6 | % is at the home position,
7 | % thetalist: A list of joint coordinates.
8 | % Returns T in SE(3) representing the end-effector frame, when the joints
9 | % are at the specified coordinates (i.t.o Space Frame).
10 | % Example Inputs:
11 | %
12 | % clear; clc;
13 | % M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]];
14 | % Slist = [[0; 0; 1; 4; 0; 0], ...
15 | % [0; 0; 0; 0; 1; 0], ...
16 | % [0; 0; -1; -6; 0; -0.1]];
17 | % thetalist =[pi / 2; 3; pi];
18 | % T = FKinSpace(M, Slist, thetalist)
19 | %
20 | % Output:
21 | % T =
22 | % -0.0000 1.0000 0 -5.0000
23 | % 1.0000 0.0000 0 4.0000
24 | % 0 0 -1.0000 1.6858
25 | % 0 0 0 1.0000
26 |
27 | T = M;
28 | for i = size(thetalist): -1: 1
29 | T = simplify(MatrixExp6_sym(VecTose3(Slist(:, i)),thetalist(i))) * T;
30 | end
31 | end
--------------------------------------------------------------------------------
/contat_lib/Help/Box_to_Belt_Contact.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
11 |
12 | 
See also Force Law Documentation
22 | 23 | -------------------------------------------------------------------------------- /contat_lib/Help/Sphere_to_Tube_Contact.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 12 |
14 | 15 | 16 | 21 |
See also Force Law Documentation
22 | 23 | -------------------------------------------------------------------------------- /mr/EulerStep.m: -------------------------------------------------------------------------------- 1 | function [thetalistNext, dthetalistNext] ... 2 | = EulerStep(thetalist, dthetalist, ddthetalist, dt) 3 | % *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** 4 | % Takes thetalist: n-vector of joint variables, 5 | % dthetalist: n-vector of joint rates, 6 | % ddthetalist: n-vector of joint accelerations, 7 | % dt: The timestep delta t. 8 | % Returns thetalistNext: Vector of joint variables after dt from first 9 | % order Euler integration, 10 | % dthetalistNext: Vector of joint rates after dt from first order 11 | % Euler integration. 12 | % Example Inputs (3 Link Robot): 13 | % 14 | % clear; clc; 15 | % thetalist = [0.1; 0.1; 0.1]; 16 | % dthetalist = [0.1; 0.2; 0.3]; 17 | % ddthetalist = [2; 1.5; 1]; 18 | % dt = 0.1; 19 | % [thetalistNext, dthetalistNext] = EulerStep(thetalist, dthetalist, ... 20 | % ddthetalist, dt) 21 | % 22 | % Output: 23 | % thetalistNext = 24 | % 0.1100 25 | % 0.1200 26 | % 0.1300 27 | % dthetalistNext = 28 | % 0.3000 29 | % 0.3500 30 | % 0.4000 31 | 32 | thetalistNext = thetalist + dt * dthetalist; 33 | dthetalistNext = dthetalist + dt * ddthetalist; 34 | end -------------------------------------------------------------------------------- /contat_lib/Help/Circle_to_Ring_Contact.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 12 |
14 | 15 | 16 | 21 |
See also Force Law Documentation
22 | 23 | -------------------------------------------------------------------------------- /contat_lib/Help/Sphere_in_Sphere_Contact.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 12 |
14 | 15 | 16 | 21 |
See also Force Law Documentation
22 | -------------------------------------------------------------------------------- /contat_lib/Help/Sphere_to_Plane_Contact.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 12 |
14 | 15 | 16 | 21 |
See also Force Law Documentation
22 | 23 | -------------------------------------------------------------------------------- /contat_lib/Help/Circle_to_Circle_Contact.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 12 |
14 | 15 | 16 | 21 |
See also Force Law Documentation
22 | 23 | -------------------------------------------------------------------------------- /contat_lib/Help/Sphere_to_Sphere_Contact.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 12 |
14 | 15 | 16 | 21 |
See also Force Law Documentation
22 | 23 | -------------------------------------------------------------------------------- /contat_lib/Help/Circle_to_Finite_Line_Contact.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 12 |
14 | 15 | 16 | 21 |
See also Force Law Documentation
22 | 23 | -------------------------------------------------------------------------------- /mr/JacobianSpace.m: -------------------------------------------------------------------------------- 1 | function Js = JacobianSpace(Slist, thetalist) 2 | % *** CHAPTER 5: VELOCITY KINEMATICS AND STATICS *** 3 | % Takes Slist: The joint screw axes in the space frame when the manipulator 4 | % is at the home position, in the format of a matrix with the 5 | % screw axes as the columns, 6 | % thetalist: A list of joint coordinates. 7 | % Returns the corresponding space Jacobian (6xn real numbers). 8 | % Example Input: 9 | % 10 | % clear; clc; 11 | % Slist = [[0; 0; 1; 0; 0.2; 0.2], ... 12 | % [1; 0; 0; 2; 0; 3], ... 13 | % [0; 1; 0; 0; 2; 1], ... 14 | % [1; 0; 0; 0.2; 0.3; 0.4]]; 15 | % thetalist = [0.2; 1.1; 0.1; 1.2]; 16 | % Js = JacobianSpace(Slist, thetalist) 17 | % 18 | % Output: 19 | % Js = 20 | % 0 0.9801 -0.0901 0.9575 21 | % 0 0.1987 0.4446 0.2849 22 | % 1.0000 0 0.8912 -0.0453 23 | % 0 1.9522 -2.2164 -0.5116 24 | % 0.2000 0.4365 -2.4371 2.7754 25 | % 0.2000 2.9603 3.2357 2.2251 26 | 27 | Js = Slist; 28 | T = eye(4); 29 | for i = 2: length(thetalist) 30 | T = T * MatrixExp6(VecTose3(Slist(:, i - 1) * thetalist(i - 1))); 31 | Js(:, i) = Adjoint(T) * Slist(:, i); 32 | end 33 | end -------------------------------------------------------------------------------- /mr/MatrixExp6.m: -------------------------------------------------------------------------------- 1 | function T = MatrixExp6(se3mat) 2 | % *** CHAPTER 3: RIGID-BODY MOTIONS *** 3 | % Takes a se(3) representation of exponential coordinates. 4 | % Returns a T matrix in SE(3) that is achieved by traveling along/about the 5 | % screw axis S for a distance theta from an initial configuration T = I. 6 | % Example Input: 7 | % 8 | % clear; clc; 9 | % se3mat = [ 0, 0, 0, 0; 10 | % 0, 0, -1.5708, 2.3562; 11 | % 0, 1.5708, 0, 2.3562; 12 | % 0, 0, 0, 0] 13 | % T = MatrixExp6(se3mat) 14 | % 15 | % Output: 16 | % T = 17 | % 1.0000 0 0 0 18 | % 0 0.0000 -1.0000 -0.0000 19 | % 0 1.0000 0.0000 3.0000 20 | % 0 0 0 1.0000 21 | 22 | omgtheta = so3ToVec(se3mat(1: 3, 1: 3)); 23 | if NearZero(norm(omgtheta)) 24 | T = [eye(3), se3mat(1: 3, 4); 0, 0, 0, 1]; 25 | else 26 | [omghat, theta] = AxisAng3(omgtheta); 27 | omgmat = se3mat(1: 3, 1: 3) / theta; 28 | T = [MatrixExp3(se3mat(1: 3, 1: 3)), ... 29 | (eye(3) * theta + (1 - cos(theta)) * omgmat ... 30 | + (theta - sin(theta)) * omgmat * omgmat) ... 31 | * se3mat(1: 3, 4) / theta; 32 | 0, 0, 0, 1]; 33 | end 34 | end -------------------------------------------------------------------------------- /mr/JacobianBody.m: -------------------------------------------------------------------------------- 1 | function Jb = JacobianBody(Blist, thetalist) 2 | % *** CHAPTER 5: VELOCITY KINEMATICS AND STATICS *** 3 | % Takes Blist: The joint screw axes in the end-effector frame when the 4 | % manipulator is at the home position, in the format of a 5 | % matrix with the screw axes as the columns, 6 | % thetalist: A list of joint coordinates. 7 | % Returns the corresponding body Jacobian (6xn real numbers). 8 | % Example Input: 9 | % 10 | % clear; clc; 11 | % Blist = [[0; 0; 1; 0; 0.2; 0.2], ... 12 | % [1; 0; 0; 2; 0; 3], ... 13 | % [0; 1; 0; 0; 2; 1], ... 14 | % [1; 0; 0; 0.2; 0.3; 0.4]]; 15 | % thetalist = [0.2; 1.1; 0.1; 1.2]; 16 | % Jb = JacobianBody(Blist, thetalist) 17 | % 18 | % Output: 19 | % Jb = 20 | % -0.0453 0.9950 0 1.0000 21 | % 0.7436 0.0930 0.3624 0 22 | % -0.6671 0.0362 -0.9320 0 23 | % 2.3259 1.6681 0.5641 0.2000 24 | % -1.4432 2.9456 1.4331 0.3000 25 | % -2.0664 1.8288 -1.5887 0.4000 26 | 27 | Jb = Blist; 28 | T = eye(4); 29 | for i = length(thetalist) - 1: -1: 1 30 | T = T * MatrixExp6(VecTose3(-1 * Blist(:, i + 1) * thetalist(i + 1))); 31 | Jb(:, i) = Adjoint(T) * Blist(:, i); 32 | end 33 | end -------------------------------------------------------------------------------- /mr/JacobianSpace_sym.m: -------------------------------------------------------------------------------- 1 | function Js = JacobianSpace_sym(Slist, thetalist) 2 | % *** CHAPTER 5: VELOCITY KINEMATICS AND STATICS *** 3 | % Takes Slist: The joint screw axes in the space frame when the manipulator 4 | % is at the home position, in the format of a matrix with the 5 | % screw axes as the columns, 6 | % thetalist: A list of joint coordinates. 7 | % Returns the corresponding space Jacobian (6xn real numbers). 8 | % Example Input: 9 | % 10 | % clear; clc; 11 | % Slist = [[0; 0; 1; 0; 0.2; 0.2], ... 12 | % [1; 0; 0; 2; 0; 3], ... 13 | % [0; 1; 0; 0; 2; 1], ... 14 | % [1; 0; 0; 0.2; 0.3; 0.4]]; 15 | % thetalist = [0.2; 1.1; 0.1; 1.2]; 16 | % Js = JacobianSpace(Slist, thetalist) 17 | % 18 | % Output: 19 | % Js = 20 | % 0 0.9801 -0.0901 0.9575 21 | % 0 0.1987 0.4446 0.2849 22 | % 1.0000 0 0.8912 -0.0453 23 | % 0 1.9522 -2.2164 -0.5116 24 | % 0.2000 0.4365 -2.4371 2.7754 25 | % 0.2000 2.9603 3.2357 2.2251 26 | 27 | Js = sym(Slist); 28 | T = eye(4); 29 | for i = 2: length(thetalist) 30 | T = T * simplify(MatrixExp6_sym(VecTose3(Slist(:, i - 1)),thetalist(i - 1))); 31 | Js(:, i) = Adjoint(T) * Slist(:, i); 32 | end 33 | end -------------------------------------------------------------------------------- /mr/JacobianBody_sym.m: -------------------------------------------------------------------------------- 1 | function Jb = JacobianBody_sym(Blist, thetalist) 2 | % *** CHAPTER 5: VELOCITY KINEMATICS AND STATICS *** 3 | % Takes Blist: The joint screw axes in the end-effector frame when the 4 | % manipulator is at the home position, in the format of a 5 | % matrix with the screw axes as the columns, 6 | % thetalist: A list of joint coordinates. 7 | % Returns the corresponding body Jacobian (6xn real numbers). 8 | % Example Input: 9 | % 10 | % clear; clc; 11 | % Blist = [[0; 0; 1; 0; 0.2; 0.2], ... 12 | % [1; 0; 0; 2; 0; 3], ... 13 | % [0; 1; 0; 0; 2; 1], ... 14 | % [1; 0; 0; 0.2; 0.3; 0.4]]; 15 | % thetalist = [0.2; 1.1; 0.1; 1.2]; 16 | % Jb = JacobianBody(Blist, thetalist) 17 | % 18 | % Output: 19 | % Jb = 20 | % -0.0453 0.9950 0 1.0000 21 | % 0.7436 0.0930 0.3624 0 22 | % -0.6671 0.0362 -0.9320 0 23 | % 2.3259 1.6681 0.5641 0.2000 24 | % -1.4432 2.9456 1.4331 0.3000 25 | % -2.0664 1.8288 -1.5887 0.4000 26 | 27 | Jb = sym(Blist); 28 | T = eye(4); 29 | for i = length(thetalist) - 1: -1: 1 30 | T = T * simplify(MatrixExp6_sym(VecTose3(Blist(:, i + 1)),-1*thetalist(i + 1))); 31 | Jb(:, i) = Adjoint(T) * Blist(:, i); 32 | end 33 | end -------------------------------------------------------------------------------- /inverse_kinematics.m: -------------------------------------------------------------------------------- 1 | function [inv_s,inv_u,inv_k] = inverse_kinematics(p, body) 2 | % from center of the shoulder 3 | x = p(1); 4 | y = p(2); 5 | z = p(3); 6 | l1 = body.upper_length; 7 | l2 = body.lower_length; 8 | 9 | tmp = y/sqrt(y*y+z*z); 10 | if tmp > 1 11 | tmp = 1; 12 | elseif tmp < -1 13 | tmp = -1; 14 | end 15 | inv_s = asin(tmp); 16 | tmp = (x*x+y*y+z*z-l1*l1-l2*l2)/(2*l1*l2); 17 | if tmp > 1 18 | tmp = 1; 19 | elseif tmp < -1 20 | tmp = -1; 21 | end 22 | inv_k = -acos(tmp); % always assume knee is minus 23 | 24 | if abs(inv_k) > 1e-4 25 | if abs(inv_s) > 1e-4 26 | temp1 = y/sin(inv_s)+(l2*cos(inv_k)+l1)*x/l2/sin(inv_k); 27 | else 28 | temp1 = -z/cos(inv_s)+(l2*cos(inv_k)+l1)*x/l2/sin(inv_k); 29 | end 30 | temp2 = -l2*sin(inv_k)-(l2*cos(inv_k)+l1)*(l2*cos(inv_k)+l1)/(l2*sin(inv_k)); 31 | tmp = temp1/temp2; 32 | if tmp > 1 33 | tmp = 1; 34 | elseif tmp < -1 35 | tmp = -1; 36 | end 37 | inv_u = asin(tmp); 38 | else 39 | tmp = x/(-l1-l2); 40 | if tmp > 1 41 | tmp = 1; 42 | elseif tmp < -1 43 | tmp = -1; 44 | end 45 | inv_u = asin(tmp); 46 | end 47 | 48 | end -------------------------------------------------------------------------------- /contat_lib/Help/Force_Laws.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 12 |
14 |
15 |
16 |
21 | Add your own in Simulink: Edit Custom Force Law
22 |See also Friction Law Documentation
23 | 24 | -------------------------------------------------------------------------------- /init_quad_new_leg.m: -------------------------------------------------------------------------------- 1 | addpath(genpath(fullfile(pwd,'contat_lib'))); 2 | %parameters for ground contract 3 | ground.stiff = 5e3; 4 | ground.damp = 1000; 5 | ground.height = 0.4; 6 | 7 | %body size 8 | body.x_length = 0.6; 9 | body.y_length = 0.3; 10 | body.z_length = 0.15; 11 | body.shoulder_size = 0.07; 12 | body.upper_length = 0.16; 13 | body.lower_length = 0.25; 14 | body.foot_radius = 0.035; 15 | body.shoulder_distance = 0.2; 16 | 17 | % parameters for leg control 18 | ctrl.pos_kp = 30; 19 | ctrl.pos_ki = 0; 20 | ctrl.pos_kd = 0; 21 | ctrl.vel_kp = 4.4; 22 | ctrl.vel_ki = 0; 23 | ctrl.vel_kd = 0; 24 | 25 | % parameters for high level plan 26 | planner.touch_down_height = body.foot_radius; % from foot center to ground 27 | 28 | planner.stand_s = 0; 29 | planner.stand_u = 35*pi/180; 30 | planner.stand_k = -60*pi/180; 31 | stance_pos = forward_kinematics(planner.stand_s, planner.stand_u, planner.stand_k, body); 32 | planner.stand_height = stance_pos(3); 33 | 34 | planner.flight_height = 0.6*planner.stand_height;% when leg is flying in the air 35 | 36 | % gait control parameters 37 | planner.time_circle = 0.5; 38 | 39 | 40 | %robot weight 41 | body_weight = 600*body.x_length*body.y_length*body.z_length; 42 | should_weight = 660*0.07^3; 43 | upperleg_weight = 660*0.04*0.04*body.upper_length; 44 | lowerleg_weight = 660*0.04*0.04*body.lower_length; 45 | foot_weight = 1000*4/3*pi*body.foot_radius^3; 46 | 47 | total_weight = body_weight + 4*(should_weight+upperleg_weight+lowerleg_weight+foot_weight) 48 | -------------------------------------------------------------------------------- /quad_whole_body_traj_gen.m: -------------------------------------------------------------------------------- 1 | addpath('mr'); 2 | %% init necessary parameters 3 | % init parameter 4 | init_quad_whole_body_ctrl; 5 | 6 | % formulate optimal stance on flat ground 7 | stance_origin = [0; 0; quad_param.leg_l2; ... % pos x y z, 8 | 0; 0; 0; ... % orientation roll, pitch, yaw 9 | 0; 0; 90/180*pi;... % leg1 angles 10 | 0; 0; 90/180*pi;... % leg2 angles 11 | 0; 0; 90/180*pi;... % leg3 angles 12 | 0; 0; 90/180*pi;... % leg4 angles 13 | 0;0;quad_param.total_mass*quad_param.g/quad_param.leg_num;... % foot 1 force 14 | 0;0;quad_param.total_mass*quad_param.g/quad_param.leg_num;... % foot 2 force 15 | 0;0;quad_param.total_mass*quad_param.g/quad_param.leg_num;... % foot 3 force 16 | 0;0;quad_param.total_mass*quad_param.g/quad_param.leg_num]; % foot 4 force 17 | 18 | stance_tgt = [0; 0; quad_param.leg_l2; ... % pos x y z, 19 | 0; 0; 0; ... % orientation roll, pitch, yaw 20 | 0; 0; 90/180*pi;... % leg1 angles 21 | 0; 0; 90/180*pi;... % leg2 angles 22 | 0; 0; 90/180*pi;... % leg3 angles 23 | 0; 0; 90/180*pi;... % leg4 angles 24 | 0;0;quad_param.total_mass*quad_param.g/quad_param.leg_num;... % foot 1 force 25 | 0;0;quad_param.total_mass*quad_param.g/quad_param.leg_num;... % foot 2 force 26 | 0;0;quad_param.total_mass*quad_param.g/quad_param.leg_num;... % foot 3 force 27 | 0;0;quad_param.total_mass*quad_param.g/quad_param.leg_num]; % foot 4 force -------------------------------------------------------------------------------- /mr/GravityForces.m: -------------------------------------------------------------------------------- 1 | function grav = GravityForces(thetalist, g, Mlist, Glist, Slist) 2 | % *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** 3 | % Takes thetalist: A list of joint variables, 4 | % g: 3-vector for gravitational acceleration, 5 | % Mlist: List of link frames i relative to i-1 at the home position, 6 | % Glist: Spatial inertia matrices Gi of the links, 7 | % Slist: Screw axes Si of the joints in a space frame, in the format 8 | % of a matrix with the screw axes as the columns. 9 | % Returns grav: The joint forces/torques required to overcome gravity at 10 | % thetalist 11 | % This function calls InverseDynamics with Ftip = 0, dthetalist = 0, and 12 | % ddthetalist = 0. 13 | % Example Input (3 Link Robot): 14 | % 15 | % clear; clc; 16 | % thetalist = [0.1; 0.1; 0.1]; 17 | % g = [0; 0; -9.8]; 18 | % M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; 19 | % M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; 20 | % M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; 21 | % M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; 22 | % G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); 23 | % G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); 24 | % G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); 25 | % Glist = cat(3, G1, G2, G3); 26 | % Mlist = cat(3, M01, M12, M23, M34); 27 | % Slist = [[1; 0; 1; 0; 1; 0], ... 28 | % [0; 1; 0; -0.089; 0; 0], ... 29 | % [0; 1; 0; -0.089; 0; 0.425]]; 30 | % grav = GravityForces(thetalist, g, Mlist, Glist, Slist) 31 | % 32 | % Output: 33 | % grav = 34 | % 28.4033 35 | % -37.6409 36 | % -5.4416 37 | 38 | n = size(thetalist, 1); 39 | grav = InverseDynamics(thetalist, zeros(n, 1), zeros(n, 1) ,g, ... 40 | [0; 0; 0; 0; 0; 0], Mlist, Glist, Slist); 41 | end -------------------------------------------------------------------------------- /mr/VelQuadraticForces.m: -------------------------------------------------------------------------------- 1 | function c = VelQuadraticForces(thetalist, dthetalist, Mlist, Glist, Slist) 2 | % *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** 3 | % Takes thetalist: A list of joint variables, 4 | % dthetalist: A list of joint rates, 5 | % Mlist: List of link frames i relative to i-1 at the home position, 6 | % Glist: Spatial inertia matrices Gi of the links, 7 | % Slist: Screw axes Si of the joints in a space frame, in the format 8 | % of a matrix with the screw axes as the columns, 9 | % Returns c: The vector c(thetalist,dthetalist) of Coriolis and centripetal 10 | % terms for a given thetalist and dthetalist. 11 | % This function calls InverseDynamics with g = 0, Ftip = 0, and 12 | % ddthetalist = 0. 13 | % Example Input (3 Link Robot): 14 | % 15 | % clear; clc; 16 | % thetalist = [0.1; 0.1; 0.1]; 17 | % dthetalist = [0.1; 0.2; 0.3]; 18 | % M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; 19 | % M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; 20 | % M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; 21 | % M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; 22 | % G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); 23 | % G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); 24 | % G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); 25 | % Glist = cat(3, G1, G2, G3); 26 | % Mlist = cat(3, M01, M12, M23, M34); 27 | % Slist = [[1; 0; 1; 0; 1; 0], ... 28 | % [0; 1; 0; -0.089; 0; 0], ... 29 | % [0; 1; 0; -0.089; 0; 0.425]]; 30 | % c = VelQuadraticForces(thetalist, dthetalist, Mlist, Glist, Slist) 31 | % 32 | % Output: 33 | % c = 34 | % 0.2645 35 | % -0.0551 36 | % -0.0069 37 | 38 | c = InverseDynamics(thetalist, dthetalist, ... 39 | zeros(size(thetalist, 1), 1), [0; 0; 0], ... 40 | [0; 0; 0; 0; 0; 0], Mlist, Glist, Slist); 41 | end -------------------------------------------------------------------------------- /mr/EndEffectorForces.m: -------------------------------------------------------------------------------- 1 | function JTFtip = EndEffectorForces(thetalist, Ftip, Mlist, Glist, Slist) 2 | % *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** 3 | % Takes thetalist: A list of joint variables, 4 | % Ftip: Spatial force applied by the end-effector expressed in frame 5 | % {n+1}, 6 | % Mlist: List of link frames i relative to i-1 at the home position, 7 | % Glist: Spatial inertia matrices Gi of the links, 8 | % Slist: Screw axes Si of the joints in a space frame, in the format 9 | % of a matrix with screw axes as the columns, 10 | % Returns JTFtip: The joint forces and torques required only to create the 11 | % end-effector force Ftip. 12 | % This function calls InverseDynamics with g = 0, dthetalist = 0, and 13 | % ddthetalist = 0. 14 | % Example Input (3 Link Robot): 15 | % 16 | % clear; clc; 17 | % thetalist = [0.1; 0.1; 0.1]; 18 | % Ftip = [1; 1; 1; 1; 1; 1]; 19 | % M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; 20 | % M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; 21 | % M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; 22 | % M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; 23 | % G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); 24 | % G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); 25 | % G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); 26 | % Glist = cat(3, G1, G2, G3); 27 | % Mlist = cat(3, M01, M12, M23, M34); 28 | % Slist = [[1; 0; 1; 0; 1; 0], ... 29 | % [0; 1; 0; -0.089; 0; 0], ... 30 | % [0; 1; 0; -0.089; 0; 0.425]]; 31 | % JTFtip = EndEffectorForces(thetalist, Ftip, Mlist, Glist, Slist) 32 | % 33 | % Output: 34 | % JTFtip = 35 | % 1.4095 36 | % 1.8577 37 | % 1.3924 38 | 39 | n = size(thetalist, 1); 40 | JTFtip = InverseDynamics(thetalist, zeros(n, 1), zeros(n, 1), ... 41 | [0; 0; 0], Ftip, Mlist, Glist, Slist); 42 | end -------------------------------------------------------------------------------- /mr/JointTrajectory.m: -------------------------------------------------------------------------------- 1 | function traj = JointTrajectory(thetastart, thetaend, Tf, N, method) 2 | % *** CHAPTER 9: TRAJECTORY GENERATION *** 3 | % Takes thetastart: The initial joint variables, 4 | % thetaend: The final joint variables, 5 | % Tf: Total time of the motion in seconds from rest to rest, 6 | % N: The number of points N > 1 (Start and stop) in the discrete 7 | % representation of the trajectory, 8 | % method: The time-scaling method, where 3 indicates cubic 9 | % (third-order polynomial) time scaling and 5 indicates 10 | % quintic (fifth-order polynomial) time scaling. 11 | % Returns traj: A trajectory as an N x n matrix, where each row is an 12 | % n-vector of joint variables at an instant in time. The 13 | % first row is thetastart and the Nth row is thetaend . The 14 | % elapsed time between each row is Tf/(N - 1). 15 | % The returned trajectory is a straight-line motion in joint space. 16 | % Example Input: 17 | % 18 | % clear; clc; 19 | % thetastart = [1; 0; 0; 1; 1; 0.2; 0; 1]; 20 | % thetaend = [1.2; 0.5; 0.6; 1.1; 2;2; 0.9; 1]; 21 | % Tf = 4; 22 | % N = 6; 23 | % method = 3; 24 | % traj = JointTrajectory(thetastart, thetaend, Tf, N, method) 25 | % 26 | % Output: 27 | % traj = 28 | % 1.0000 0 0 1.0000 1.0000 0.2000 0 1.0000 29 | % 1.0208 0.0520 0.0624 1.0104 1.1040 0.3872 0.0936 1.0000 30 | % 1.0704 0.1760 0.2112 1.0352 1.3520 0.8336 0.3168 1.0000 31 | % 1.1296 0.3240 0.3888 1.0648 1.6480 1.3664 0.5832 1.0000 32 | % 1.1792 0.4480 0.5376 1.0896 1.8960 1.8128 0.8064 1.0000 33 | % 1.2000 0.5000 0.6000 1.1000 2.0000 2.0000 0.9000 1.0000 34 | 35 | timegap = Tf / (N - 1); 36 | traj = zeros(size(thetastart, 1), N); 37 | for i = 1: N 38 | if method == 3 39 | s = CubicTimeScaling(Tf, timegap * (i - 1)); 40 | else 41 | s = QuinticTimeScaling(Tf, timegap * (i - 1)); 42 | end 43 | traj(:, i) = thetastart + s * (thetaend - thetastart); 44 | end 45 | traj = traj'; 46 | end -------------------------------------------------------------------------------- /mr/MassMatrix.m: -------------------------------------------------------------------------------- 1 | function M = MassMatrix(thetalist, Mlist, Glist, Slist) 2 | % *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** 3 | % Takes thetalist: A list of joint variables, 4 | % Mlist: List of link frames i relative to i-1 at the home position, 5 | % Glist: Spatial inertia matrices Gi of the links, 6 | % Slist: Screw axes Si of the joints in a space frame, in the format 7 | % of a matrix with the screw axes as the columns. 8 | % Returns M: The numerical inertia matrix M(thetalist) of an n-joint serial 9 | % chain at the given configuration thetalist. 10 | % This function calls InverseDynamics n times, each time passing a 11 | % ddthetalist vector with a single element equal to one and all other 12 | % inputs set to zero. Each call of InverseDynamics generates a single 13 | % column, and these columns are assembled to create the inertia matrix. 14 | % Example Input (3 Link Robot): 15 | % 16 | % clear; clc; 17 | % thetalist = [0.1; 0.1; 0.1]; 18 | % M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; 19 | % M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; 20 | % M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; 21 | % M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; 22 | % G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); 23 | % G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); 24 | % G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); 25 | % Glist = cat(3, G1, G2, G3); 26 | % Mlist = cat(3, M01, M12, M23, M34); 27 | % Slist = [[1; 0; 1; 0; 1; 0], ... 28 | % [0; 1; 0; -0.089; 0; 0], ... 29 | % [0; 1; 0; -0.089; 0; 0.425]]; 30 | % M = MassMatrix(thetalist, Mlist, Glist, Slist) 31 | % 32 | % Output: 33 | % M = 34 | % 22.5433 -0.3071 -0.0072 35 | % -0.3071 1.9685 0.4322 36 | % -0.0072 0.4322 0.1916 37 | 38 | n = size(thetalist, 1); 39 | M = zeros(n); 40 | for i = 1: n 41 | ddthetalist = zeros(n, 1); 42 | ddthetalist(i) = 1; 43 | M(:, i) = InverseDynamics(thetalist, zeros(n, 1), ddthetalist, ... 44 | [0; 0; 0], [0; 0; 0; 0; 0; 0],Mlist, ... 45 | Glist, Slist); 46 | end 47 | end -------------------------------------------------------------------------------- /mr/ScrewTrajectory.m: -------------------------------------------------------------------------------- 1 | function traj = ScrewTrajectory(Xstart, Xend, Tf, N, method) 2 | % *** CHAPTER 9: TRAJECTORY GENERATION *** 3 | % Takes Xstart: The initial end-effector configuration, 4 | % Xend: The final end-effector configuration, 5 | % Tf: Total time of the motion in seconds from rest to rest, 6 | % N: The number of points N > 1 (Start and stop) in the discrete 7 | % representation of the trajectory, 8 | % method: The time-scaling method, where 3 indicates cubic 9 | % (third-order polynomial) time scaling and 5 indicates 10 | % quintic (fifth-order polynomial) time scaling. 11 | % Returns traj: The discretized trajectory as a list of N matrices in SE(3) 12 | % separated in time by Tf/(N-1). The first in the list is 13 | % Xstart and the Nth is Xend . 14 | % This function calculates a trajectory corresponding to the screw motion 15 | % about a space screw axis. 16 | % Example Input: 17 | % 18 | % clear; clc; 19 | % Xstart = [[1 ,0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 1]; [0, 0, 0, 1]]; 20 | % Xend = [[0, 0, 1, 0.1]; [1, 0, 0, 0]; [0, 1, 0, 4.1]; [0, 0, 0, 1]]; 21 | % Tf = 5; 22 | % N = 4; 23 | % method = 3; 24 | % traj = ScrewTrajectory(Xstart, Xend, Tf, N, method) 25 | % 26 | % Output: 27 | % traj = 28 | % 1.0000 0 0 1.0000 29 | % 0 1.0000 0 0 30 | % 0 0 1.0000 1.0000 31 | % 0 0 0 1.0000 32 | % 33 | % 0.9041 -0.2504 0.3463 0.4410 34 | % 0.3463 0.9041 -0.2504 0.5287 35 | % -0.2504 0.3463 0.9041 1.6007 36 | % 0 0 0 1.0000 37 | % 38 | % 0.3463 -0.2504 0.9041 -0.1171 39 | % 0.9041 0.3463 -0.2504 0.4727 40 | % -0.2504 0.9041 0.3463 3.2740 41 | % 0 0 0 1.0000 42 | % 43 | % -0.0000 0.0000 1.0000 0.1000 44 | % 1.0000 -0.0000 0.0000 -0.0000 45 | % 0.0000 1.0000 -0.0000 4.1000 46 | % 0 0 0 1.0000 47 | 48 | timegap = Tf / (N - 1); 49 | traj = cell(1, N); 50 | for i = 1: N 51 | if method == 3 52 | s = CubicTimeScaling(Tf, timegap * (i - 1)); 53 | else 54 | s = QuinticTimeScaling(Tf, timegap * (i - 1)); 55 | end 56 | traj{i} = Xstart * MatrixExp6(MatrixLog6(TransInv(Xstart) * Xend) * s); 57 | end 58 | end 59 | 60 | -------------------------------------------------------------------------------- /mr/CartesianTrajectory.m: -------------------------------------------------------------------------------- 1 | function traj = CartesianTrajectory(Xstart, Xend, Tf, N, method) 2 | % *** CHAPTER 9: TRAJECTORY GENERATION *** 3 | % Takes Xstart: The initial end-effector configuration, 4 | % Xend: The final end-effector configuration, 5 | % Tf: Total time of the motion in seconds from rest to rest, 6 | % N: The number of points N > 1 (Start and stop) in the discrete 7 | % representation of the trajectory, 8 | % method: The time-scaling method, where 3 indicates cubic 9 | % (third-order polynomial) time scaling and 5 indicates 10 | % quintic (fifth-order polynomial) time scaling. 11 | % Returns traj: The discretized trajectory as a list of N matrices in SE(3) 12 | % separated in time by Tf/(N-1). The first in the list is 13 | % Xstart and the Nth is Xend . 14 | % This function is similar to ScrewTrajectory, except the origin of the 15 | % end-effector frame follows a straight line, decoupled from the rotational 16 | % motion. 17 | % Example Input: 18 | % 19 | % clear; clc; 20 | % Xstart = [[1, 0, 0, 1]; [0, 1, 0, 0]; [0, 0, 1, 1]; [0, 0, 0, 1]]; 21 | % Xend = [[0, 0, 1, 0.1]; [1, 0, 0, 0]; [0, 1, 0, 4.1]; [0, 0, 0, 1]]; 22 | % Tf = 5; 23 | % N = 4; 24 | % method = 5; 25 | % traj = CartesianTrajectory(Xstart, Xend, Tf, N, method) 26 | % 27 | % Output: 28 | % traj = 29 | % 1.0000 0 0 1.0000 30 | % 0 1.0000 0 0 31 | % 0 0 1.0000 1.0000 32 | % 0 0 0 1.0000 33 | % 34 | % 0.9366 -0.2140 0.2774 0.8111 35 | % 0.2774 0.9366 -0.2140 0 36 | % -0.2140 0.2774 0.9366 1.6506 37 | % 0 0 0 1.0000 38 | % 39 | % 0.2774 -0.2140 0.9366 0.2889 40 | % 0.9366 0.2774 -0.2140 0 41 | % -0.2140 0.9366 0.2774 3.4494 42 | % 0 0 0 1.0000 43 | % 44 | % -0.0000 0.0000 1.0000 0.1000 45 | % 1.0000 -0.0000 0.0000 0 46 | % 0.0000 1.0000 -0.0000 4.1000 47 | % 0 0 0 1.0000 48 | 49 | timegap = Tf / (N - 1); 50 | traj = cell(1, N); 51 | [Rstart, pstart] = TransToRp(Xstart); 52 | [Rend, pend] = TransToRp(Xend); 53 | for i = 1: N 54 | if method == 3 55 | s = CubicTimeScaling(Tf,timegap * (i - 1)); 56 | else 57 | s = QuinticTimeScaling(Tf,timegap * (i - 1)); 58 | end 59 | traj{i} ... 60 | = [Rstart * MatrixExp3(MatrixLog3(Rstart' * Rend) * s), ... 61 | pstart + s * (pend - pstart); 0, 0, 0, 1]; 62 | end 63 | end -------------------------------------------------------------------------------- /init_quad_new_leg_raibert_strategy_v2.m: -------------------------------------------------------------------------------- 1 | % The contact lib by Mathworks, may change to another lib written by 2 | % Professor Hartmut Geyer 3 | addpath(genpath(fullfile(pwd,'contat_lib'))); 4 | %parameters for ground plane geometry and contract 5 | ground.stiff = 5e4; 6 | ground.damp = 1000; 7 | ground.height = 0.4; 8 | 9 | %robot body size 10 | body.x_length = 0.6; 11 | body.y_length = 0.3; 12 | body.z_length = 0.15; 13 | body.shoulder_size = 0.07; 14 | body.upper_length = 0.16; 15 | body.lower_length = 0.25; 16 | body.foot_radius = 0.035; 17 | body.shoulder_distance = 0.2; 18 | body.max_stretch = body.upper_length + body.lower_length; 19 | body.knee_damping = 0.1; 20 | 21 | % parameters for leg control 22 | ctrl.pos_kp = 30; 23 | ctrl.pos_ki = 0; 24 | ctrl.pos_kd = 0; 25 | ctrl.vel_kp = 4.4; 26 | ctrl.vel_ki = 0; 27 | ctrl.vel_kd = 0.0; 28 | 29 | % parameters for high level plan 30 | planner.touch_down_height = body.foot_radius; % from foot center to ground 31 | 32 | planner.stand_s = 0; 33 | planner.stand_u = 30*pi/180; 34 | planner.stand_k = -60*pi/180; 35 | stance_pos = forward_kinematics(planner.stand_s, planner.stand_u, planner.stand_k, body); 36 | planner.stand_height = stance_pos(3); 37 | 38 | planner.flight_height = 1.1*planner.stand_height;% when leg is flying in the air 39 | 40 | % gait control parameters 41 | planner.time_circle = 3; 42 | planner.swing_ang = 12/180*pi; 43 | planner.init_shake_ang = 3/180*pi; 44 | 45 | %some gait control goals 46 | planner.tgt_body_ang = 0; 47 | planner.tgt_body_vx = 0.3; % tested 0.15-0.45 48 | planner.Ts = 0.1; % for x_direction leg place 49 | planner.Kv = 0.3; % for x_dreiction leg place 50 | 51 | 52 | planner.y_Ts = 0.21; % for y_direction leg place 53 | planner.y_Kv = -0.34; % for y_direction leg place 54 | 55 | % state transition thredsholds 56 | planner.state0_vel_thres = 0.05; 57 | % the sample time of the ctrl block is set to be 0.0025 (400Hz); 58 | planner.state0_trans_thres = 300; %(0.75 seconds) 59 | planner.state0_swing_ang = 10*pi/180; 60 | planner.state0_swing_T = 1200; 61 | planner.state12_trans_speed = 0.2; 62 | planner.leg_swing_time = 70; %(use 0.25s to generate a motion) 63 | 64 | %robot weight 65 | body_weight = 600*body.x_length*body.y_length*body.z_length; 66 | leg_density = 660; 67 | should_weight = leg_density*0.07^3; 68 | upperleg_weight = leg_density*0.04*0.04*body.upper_length; 69 | lowerleg_weight = leg_density*0.04*0.04*body.lower_length; 70 | foot_weight = 1000*4/3*pi*body.foot_radius^3; 71 | 72 | total_weight = body_weight + 4*(should_weight+upperleg_weight+lowerleg_weight+foot_weight); 73 | 74 | body_inertia = diag([0.151875;0.516375;0.6075]); 75 | -------------------------------------------------------------------------------- /mr/ForwardDynamics.m: -------------------------------------------------------------------------------- 1 | function ddthetalist = ForwardDynamics(thetalist, dthetalist, taulist, ... 2 | g, Ftip, Mlist, Glist, Slist) 3 | % *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** 4 | % Takes thetalist: A list of joint variables, 5 | % dthetalist: A list of joint rates, 6 | % taulist: An n-vector of joint forces/torques, 7 | % g: Gravity vector g, 8 | % Ftip: Spatial force applied by the end-effector expressed in frame 9 | % {n+1}, 10 | % Mlist: List of link frames i relative to i-1 at the home position, 11 | % Glist: Spatial inertia matrices Gi of the links, 12 | % Slist: Screw axes Si of the joints in a space frame, in the format 13 | % of a matrix with the screw axes as the columns, 14 | % Returns ddthetalist: The resulting joint accelerations. 15 | % This function computes ddthetalist by solving: 16 | % Mlist(thetalist) * ddthetalist = taulist - c(thetalist,dthetalist) ... 17 | % - g(thetalist) - Jtr(thetalist) * Ftip 18 | % Example Input (3 Link Robot): 19 | % 20 | % clear; clc; 21 | % thetalist = [0.1; 0.1; 0.1]; 22 | % dthetalist = [0.1; 0.2; 0.3]; 23 | % taulist = [0.5; 0.6; 0.7]; 24 | % g = [0; 0; -9.8]; 25 | % Ftip = [1; 1; 1; 1; 1; 1]; 26 | % M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; 27 | % M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; 28 | % M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; 29 | % M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; 30 | % G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); 31 | % G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); 32 | % G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); 33 | % Glist = cat(3, G1, G2, G3); 34 | % Mlist = cat(3, M01, M12, M23, M34); 35 | % Slist = [[1; 0; 1; 0; 1; 0], ... 36 | % [0; 1; 0; -0.089; 0; 0], ... 37 | % [0; 1; 0; -0.089; 0; 0.425]]; 38 | % ddthetalist = ForwardDynamics(thetalist, dthetalist, taulist, g, ... 39 | % Ftip, Mlist, Glist, Slist) 40 | % 41 | % Output: 42 | % ddthetalist = 43 | % -0.9739 44 | % 25.5847 45 | % -32.9150 46 | 47 | ddthetalist = MassMatrix(thetalist, Mlist, Glist, Slist) ... 48 | \ (taulist - VelQuadraticForces(thetalist, dthetalist, ... 49 | Mlist, Glist, Slist) ... 50 | - GravityForces(thetalist, g, Mlist, Glist, Slist) ... 51 | - EndEffectorForces(thetalist, Ftip, Mlist, Glist, ... 52 | Slist)); 53 | end -------------------------------------------------------------------------------- /mr/IKinBody.m: -------------------------------------------------------------------------------- 1 | function [thetalist, success] = IKinBody(Blist, M, T, thetalist0, eomg, ev) 2 | % *** CHAPTER 6: INVERSE KINEMATICS *** 3 | % Takes Blist: The joint screw axes in the end-effector frame when the 4 | % manipulator is at the home position, in the format of a 5 | % matrix with the screw axes as the columns, 6 | % M: The home configuration of the end-effector, 7 | % T: The desired end-effector configuration Tsd, 8 | % thetalist0: An initial guess of joint angles that are close to 9 | % satisfying Tsd, 10 | % eomg: A small positive tolerance on the end-effector orientation 11 | % error. The returned joint angles must give an end-effector 12 | % orientation error less than eomg, 13 | % ev: A small positive tolerance on the end-effector linear position 14 | % error. The returned joint angles must give an end-effector 15 | % position error less than ev. 16 | % Returns thetalist: Joint angles that achieve T within the specified 17 | % tolerances, 18 | % success: A logical value where TRUE means that the function found 19 | % a solution and FALSE means that it ran through the set 20 | % number of maximum iterations without finding a solution 21 | % within the tolerances eomg and ev. 22 | % Uses an iterative Newton-Raphson root-finding method. 23 | % The maximum number of iterations before the algorithm is terminated has 24 | % been hardcoded in as a variable called maxiterations. It is set to 20 at 25 | % the start of the function, but can be changed if needed. 26 | % Example Inputs: 27 | % 28 | % clear; clc; 29 | % Blist = [[0; 0; -1; 2; 0; 0], [0; 0; 0; 0; 1; 0], [0; 0; 1; 0; 0; 0.1]]; 30 | % M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]]; 31 | % T = [[0, 1, 0, -5]; [1, 0, 0, 4]; [0, 0, -1, 1.6858]; [0, 0, 0, 1]]; 32 | % thetalist0 = [1.5; 2.5; 3]; 33 | % eomg = 0.01; 34 | % ev = 0.001; 35 | % [thetalist, success] = IKinBody(Blist, M, T, thetalist0, eomg, ev) 36 | % 37 | % Output: 38 | % thetalist = 39 | % 1.5707 40 | % 2.9997 41 | % 3.1415 42 | % success = 43 | % 1 44 | 45 | thetalist = thetalist0; 46 | i = 0; 47 | maxiterations = 20; 48 | Vb = se3ToVec(MatrixLog6(TransInv(FKinBody(M, Blist, thetalist)) * T)); 49 | err = norm(Vb(1: 3)) > eomg || norm(Vb(4: 6)) > ev; 50 | while err && i < maxiterations 51 | thetalist = thetalist + pinv(JacobianBody(Blist, thetalist)) * Vb; 52 | i = i + 1; 53 | Vb = se3ToVec(MatrixLog6(TransInv(FKinBody(M, Blist, thetalist)) * T)); 54 | err = norm(Vb(1: 3)) > eomg || norm(Vb(4: 6)) > ev; 55 | end 56 | success = ~ err; 57 | end -------------------------------------------------------------------------------- /init_quad_new_leg_raibert_strategy_return_map.m: -------------------------------------------------------------------------------- 1 | % The contact lib by Mathworks, may change to another lib written by 2 | % Professor Hartmut Geyer 3 | addpath(genpath(fullfile(pwd,'contat_lib'))); 4 | %parameters for ground plane geometry and contract 5 | ground.stiff = 5e4; 6 | ground.damp = 1000; 7 | ground.height = 0.4; 8 | 9 | %robot body size 10 | body.x_length = 0.6; 11 | body.y_length = 0.3; 12 | body.z_length = 0.15; 13 | body.shoulder_size = 0.07; 14 | body.upper_length = 0.16; 15 | body.lower_length = 0.25; 16 | body.foot_radius = 0.035; 17 | body.shoulder_distance = 0.2; 18 | body.max_stretch = body.upper_length + body.lower_length; 19 | body.knee_damping = 0.1; 20 | 21 | % parameters for leg control 22 | ctrl.pos_kp = 30; 23 | ctrl.pos_ki = 0; 24 | ctrl.pos_kd = 0; 25 | ctrl.vel_kp = 4.4; 26 | ctrl.vel_ki = 0; 27 | ctrl.vel_kd = 0.0; 28 | 29 | % parameters for high level plan 30 | planner.touch_down_height = body.foot_radius; % from foot center to ground 31 | 32 | planner.stand_s = 0; 33 | planner.stand_u = 30*pi/180; 34 | planner.stand_k = -60*pi/180; 35 | stance_pos = forward_kinematics(planner.stand_s, planner.stand_u, planner.stand_k, body); 36 | planner.stand_height = stance_pos(3); 37 | 38 | planner.flight_height = 1.1*planner.stand_height;% when leg is flying in the air 39 | 40 | % gait control parameters 41 | planner.time_circle = 3; 42 | planner.swing_ang = 12/180*pi; 43 | planner.init_shake_ang = 3/180*pi; 44 | 45 | %some gait control goals 46 | planner.tgt_body_ang = 0; 47 | planner.tgt_body_vx = 0.3; % tested 0.15-0.45 48 | planner.Ts = 0.1; % for x_direction leg place 49 | planner.Kv = 0.3; % for x_dreiction leg place 50 | 51 | 52 | planner.y_Ts = 0.21; % for y_direction leg place 53 | planner.y_Kv = -0.34; % for y_direction leg place 54 | 55 | % state transition thredsholds 56 | planner.state0_vel_thres = 0.05; 57 | % the sample time of the ctrl block is set to be 0.0025 (400Hz); 58 | planner.state0_trans_thres = 300; %(0.75 seconds) 59 | planner.state0_swing_ang = 10*pi/180; 60 | planner.state0_swing_T = 1200; 61 | planner.state12_trans_speed = 0.2; 62 | planner.leg_swing_time = 70; %(use 0.25s to generate a motion) 63 | 64 | % to generate return map 65 | planner.init_speed = 0.05; 66 | 67 | %robot weight 68 | body_weight = 600*body.x_length*body.y_length*body.z_length; 69 | leg_density = 660; 70 | should_weight = leg_density*0.07^3; 71 | upperleg_weight = leg_density*0.04*0.04*body.upper_length; 72 | lowerleg_weight = leg_density*0.04*0.04*body.lower_length; 73 | foot_weight = 1000*4/3*pi*body.foot_radius^3; 74 | 75 | total_weight = body_weight + 4*(should_weight+upperleg_weight+lowerleg_weight+foot_weight); 76 | 77 | body_inertia = diag([0.151875;0.516375;0.6075]); 78 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Starting Jun 2020, I will reactivate this repo and use it to help a onging research 2 | Revise a number of conventions 3 | 4 | 5 | 6 | ---------------------------------------------------------- 7 | - Old Code And Old README - 8 | ---------------------------------------------------------- 9 | 10 | Two years ago after reading "Legged Robots that Balance", I started to work on a MATLAB simulation of a quadruped robot. Initially this project is put in my "toy_code" repo. This semester I will use this quadruped robot for two of my CMU course projects. So it is necessary to create a dedicated repository for it. 11 | 12 | [](https://www.youtube.com/watch?v=ByDtmprLmHg) 13 | ---------- 14 | 15 | General Principle 16 | 17 | Simulate a walking quadruped robot is easy (But remember all simulations are wrong), first construct body and legs, then carefully understand the frame transformations among them to get forward and inverse kinematics. So the robot knows how to place its feet. 18 | 19 | Then find a good method to simulate the contact forces between a foot and the ground. This is proved to be very difficult because few exisiting contact models are precise enough compared to real world scenario. Besides, bad contact force model can make simulation run slower, for the contact force will make the entire simulation model "stiff". In the current implementation, I use [Simscape Multibody Contact Forces Library][1] created by Steve Miller. 20 | 21 | Then design a gait planner to move feet in a periodic manner. 22 | 23 | 24 | ---------- 25 | 26 | 27 | Helper code: 28 | 29 | 1. *forward_kinematics.m* Calculate forward kinematics of the robot leg (Each leg is a simple three-joint manipulator. From shoulder to foot tip, jonts are labelled as "shoulder", "upper" and "knee". Then in the forward kinematics they are abbreviated as "s" "u" and "k") 30 | 2. *inverse_kinematics.m* Calculate inverse kinematics 31 | 3. *init_quad_new_leg.m* Initialize some parameters 32 | 33 | 34 | **quad_XXXX.slx** will be the name of different models. I will make sure all **quad_XXXX.sx** file has a corresponding **init_quad_XXXX.m** initialization file. 35 | 36 | By Dec-6-2018, the best running model is 37 | 38 | *quad_new_leg_raibert_strategy_v2.slx* 39 | 40 | and its init file 41 | 42 | *init_quad_new_leg_raibert_strategy_v2.m* 43 | 44 | 45 | 46 | 47 | ---------- 48 | Notice: 49 | 50 | 1. Check [this post][2] to properly use contact forces library 51 | 52 | 53 | [1]: https://www.mathworks.com/matlabcentral/fileexchange/47417-simscape-multibody-contact-forces-library 54 | [2]: https://www.mathworks.com/matlabcentral/answers/378561-rigidly-connected-port-error-with-simscape-multibody-contact-forces-library 55 | -------------------------------------------------------------------------------- /mr/IKinSpace.m: -------------------------------------------------------------------------------- 1 | function [thetalist, success] ... 2 | = IKinSpace(Slist, M, T, thetalist0, eomg, ev) 3 | % *** CHAPTER 6: INVERSE KINEMATICS *** 4 | % Takes Slist: The joint screw axes in the space frame when the manipulator 5 | % is at the home position, in the format of a matrix with the 6 | % screw axes as the columns, 7 | % M: The home configuration of the end-effector, 8 | % T: The desired end-effector configuration Tsd, 9 | % thetalist0: An initial guess of joint angles that are close to 10 | % satisfying Tsd, 11 | % eomg: A small positive tolerance on the end-effector orientation 12 | % error. The returned joint angles must give an end-effector 13 | % orientation error less than eomg, 14 | % ev: A small positive tolerance on the end-effector linear position 15 | % error. The returned joint angles must give an end-effector 16 | % position error less than ev. 17 | % Returns thetalist: Joint angles that achieve T within the specified 18 | % tolerances, 19 | % success: A logical value where TRUE means that the function found 20 | % a solution and FALSE means that it ran through the set 21 | % number of maximum iterations without finding a solution 22 | % within the tolerances eomg and ev. 23 | % Uses an iterative Newton-Raphson root-finding method. 24 | % The maximum number of iterations before the algorithm is terminated has 25 | % been hardcoded in as a variable called maxiterations. It is set to 20 at 26 | % the start of the function, but can be changed if needed. 27 | % Example Inputs: 28 | % 29 | % clear; clc; 30 | % Slist = [[0; 0; 1; 4; 0; 0], ... 31 | % [0; 0; 0; 0; 1; 0], ... 32 | % [0; 0; -1; -6; 0; -0.1]]; 33 | % M = [[-1, 0, 0, 0]; [0, 1, 0, 6]; [0, 0, -1, 2]; [0, 0, 0, 1]]; 34 | % T = [[0, 1, 0, -5]; [1, 0, 0, 4]; [0, 0, -1, 1.6858]; [0, 0, 0, 1]]; 35 | % thetalist0 = [1.5; 2.5; 3]; 36 | % eomg = 0.01; 37 | % ev = 0.001; 38 | % [thetalist, success] = IKinSpace(Slist, M, T, thetalist0, eomg, ev) 39 | % 40 | % Output: 41 | % thetalist = 42 | % 1.5707 43 | % 2.9997 44 | % 3.1415 45 | % success = 46 | % 1 47 | 48 | thetalist = thetalist0; 49 | i = 0; 50 | maxiterations = 20; 51 | Tsb = FKinSpace(M, Slist, thetalist); 52 | Vs = Adjoint(Tsb) * se3ToVec(MatrixLog6(TransInv(Tsb) * T)); 53 | err = norm(Vs(1: 3)) > eomg || norm(Vs(4: 6)) > ev; 54 | while err && i < maxiterations 55 | thetalist = thetalist + pinv(JacobianSpace(Slist, thetalist)) * Vs; 56 | i = i + 1; 57 | Tsb = FKinSpace(M, Slist, thetalist); 58 | Vs = Adjoint(Tsb) * se3ToVec(MatrixLog6(TransInv(Tsb) * T)); 59 | err = norm(Vs(1: 3)) > eomg || norm(Vs(4: 6)) > ev; 60 | end 61 | success = ~ err; 62 | end -------------------------------------------------------------------------------- /mr/ComputedTorque.m: -------------------------------------------------------------------------------- 1 | function taulist = ComputedTorque(thetalist, dthetalist, eint, g, ... 2 | Mlist, Glist, Slist, thetalistd, ... 3 | dthetalistd, ddthetalistd, Kp, Ki, Kd) 4 | % *** CHAPTER 11: ROBOT CONTROL *** 5 | % Takes thetalist: n-vector of joint variables, 6 | % dthetalist: n-vector of joint rates, 7 | % eint: n-vector of the time-integral of joint errors, 8 | % g: Gravity vector g, 9 | % Mlist: List of link frames {i} relative to {i-1} at the home 10 | % position, 11 | % Glist: Spatial inertia matrices Gi of the links, 12 | % Slist: Screw axes Si of the joints in a space frame, in the format 13 | % of a matrix with the screw axes as the columns, 14 | % thetalistd: n-vector of reference joint variables, 15 | % dthetalistd: n-vector of reference joint velocities, 16 | % ddthetalistd: n-vector of reference joint accelerations, 17 | % Kp: The feedback proportional gain (identical for each joint), 18 | % Ki: The feedback integral gain (identical for each joint), 19 | % Kd: The feedback derivative gain (identical for each joint). 20 | % Returns taulist: The vector of joint forces/torques computed by the 21 | % feedback linearizing controller at the current instant. 22 | % Example Input: 23 | % 24 | % clc; clear; 25 | % thetalist = [0.1; 0.1; 0.1]; 26 | % dthetalist = [0.1; 0.2; 0.3]; 27 | % eint = [0.2; 0.2; 0.2]; 28 | % g = [0; 0; -9.8]; 29 | % M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; 30 | % M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; 31 | % M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; 32 | % M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; 33 | % G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); 34 | % G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); 35 | % G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); 36 | % Glist = cat(3, G1, G2, G3); 37 | % Mlist = cat(3, M01, M12, M23, M34); 38 | % Slist = [[1; 0; 1; 0; 1; 0], ... 39 | % [0; 1; 0; -0.089; 0; 0], ... 40 | % [0; 1; 0; -0.089; 0; 0.425]]; 41 | % thetalistd = [1; 1; 1]; 42 | % dthetalistd = [2; 1.2; 2]; 43 | % ddthetalistd = [0.1; 0.1; 0.1]; 44 | % Kp = 1.3; 45 | % Ki = 1.2; 46 | % Kd = 1.1; 47 | % taulist ... 48 | % = ComputedTorque(thetalist, dthetalist, eint, g, Mlist, Glist, Slist, ... 49 | % thetalistd, dthetalistd, ddthetalistd, Kp, Ki, Kd) 50 | % 51 | % Output: 52 | % taulist = 53 | % 133.0053 54 | % -29.9422 55 | % -3.0328 56 | 57 | e = thetalistd - thetalist; 58 | taulist ... 59 | = MassMatrix(thetalist, Mlist, Glist, Slist) ... 60 | * (Kp * e + Ki * (eint + e) + Kd * (dthetalistd - dthetalist)) ... 61 | + InverseDynamics(thetalist, dthetalist, ddthetalistd, g, ... 62 | zeros(6, 1), Mlist, Glist, Slist); 63 | end -------------------------------------------------------------------------------- /mr/InverseDynamics.m: -------------------------------------------------------------------------------- 1 | function taulist = InverseDynamics(thetalist, dthetalist, ddthetalist, ... 2 | g, Ftip, Mlist, Glist, Slist) 3 | % *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** 4 | % Takes thetalist: n-vector of joint variables, 5 | % dthetalist: n-vector of joint rates, 6 | % ddthetalist: n-vector of joint accelerations, 7 | % g: Gravity vector g, 8 | % Ftip: Spatial force applied by the end-effector expressed in frame 9 | % {n+1}, 10 | % Mlist: List of link frames {i} relative to {i-1} at the home 11 | % position, 12 | % Glist: Spatial inertia matrices Gi of the links, 13 | % Slist: Screw axes Si of the joints in a space frame, in the format 14 | % of a matrix with the screw axes as the columns. 15 | % Returns taulist: The n-vector of required joint forces/torques. 16 | % This function uses forward-backward Newton-Euler iterations to solve the 17 | % equation: 18 | % taulist = Mlist(thetalist) * ddthetalist + c(thetalist, dthetalist) ... 19 | % + g(thetalist) + Jtr(thetalist) * Ftip 20 | % Example Input (3 Link Robot): 21 | % 22 | % clear; clc; 23 | % thetalist = [0.1; 0.1; 0.1]; 24 | % dthetalist = [0.1; 0.2; 0.3]; 25 | % ddthetalist = [2; 1.5; 1]; 26 | % g = [0; 0; -9.8]; 27 | % Ftip = [1; 1; 1; 1; 1; 1]; 28 | % M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; 29 | % M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; 30 | % M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; 31 | % M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; 32 | % G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); 33 | % G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); 34 | % G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); 35 | % Glist = cat(3, G1, G2, G3); 36 | % Mlist = cat(3, M01, M12, M23, M34); 37 | % Slist = [[1; 0; 1; 0; 1; 0], ... 38 | % [0; 1; 0; -0.089; 0; 0], ... 39 | % [0; 1; 0; -0.089; 0; 0.425]]; 40 | % taulist = InverseDynamics(thetalist, dthetalist, ddthetalist, g, ... 41 | % Ftip, Mlist, Glist, Slist) 42 | % 43 | % Output: 44 | % taulist = 45 | % 74.6962 46 | % -33.0677 47 | % -3.2306 48 | 49 | n = size(thetalist, 1); 50 | Mi = eye(4); 51 | Ai = zeros(6, n); 52 | AdTi = zeros(6, 6, n + 1); 53 | Vi = zeros(6, n + 1); 54 | Vdi = zeros(6, n + 1); 55 | Vdi(4: 6, 1) = -g; 56 | AdTi(:, :, n + 1) = Adjoint(TransInv(Mlist(:, :, n + 1))); 57 | Fi = Ftip; 58 | taulist = zeros(n, 1); 59 | for i=1: n 60 | Mi = Mi * Mlist(:, :, i); 61 | Ai(:, i) = Adjoint(TransInv(Mi)) * Slist(:, i); 62 | AdTi(:, :, i) = Adjoint(MatrixExp6(VecTose3(Ai(:, i) ... 63 | * -thetalist(i))) * TransInv(Mlist(:, :, i))); 64 | Vi(:, i + 1) = AdTi(:, :, i) * Vi(:, i) + Ai(:, i) * dthetalist(i); 65 | Vdi(:, i + 1) = AdTi(:, :, i) * Vdi(:, i) ... 66 | + Ai(:, i) * ddthetalist(i) ... 67 | + ad(Vi(:, i + 1)) * Ai(:, i) * dthetalist(i); 68 | end 69 | for i = n: -1: 1 70 | Fi = AdTi(:, :, i + 1)' * Fi + Glist(:, :, i) * Vdi(:, i + 1) ... 71 | - ad(Vi(:, i + 1))' * (Glist(:, :, i) * Vi(:, i + 1)); 72 | taulist(i) = Fi' * Ai(:, i); 73 | end 74 | end -------------------------------------------------------------------------------- /mr/InverseDynamicsTrajectory.m: -------------------------------------------------------------------------------- 1 | function taumat ... 2 | = InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, ... 3 | g, Ftipmat, Mlist, Glist, Slist) 4 | % *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** 5 | % Takes thetamat: An N x n matrix of robot joint variables, 6 | % dthetamat: An N x n matrix of robot joint velocities, 7 | % ddthetamat: An N x n matrix of robot joint accelerations, 8 | % g: Gravity vector g, 9 | % Ftipmat: An N x 6 matrix of spatial forces applied by the 10 | % end-effector (If there are no tip forces, the user should 11 | % input a zero and a zero matrix will be used), 12 | % Mlist: List of link frames i relative to i-1 at the home position, 13 | % Glist: Spatial inertia matrices Gi of the links, 14 | % Slist: Screw axes Si of the joints in a space frame, in the format 15 | % of a matrix with the screw axes as the columns. 16 | % Returns taumat: The N x n matrix of joint forces/torques for the 17 | % specified trajectory, where each of the N rows is the 18 | % vector of joint forces/torques at each time step. 19 | % This function uses InverseDynamics to calculate the joint forces/torques 20 | % required to move the serial chain along the given trajectory. 21 | % Example Inputs (3 Link Robot) 22 | 23 | % clc; clear; 24 | % %Create a trajectory to follow using functions from Chapter 9 25 | % thetastart = [0; 0; 0]; 26 | % thetaend = [pi / 2; pi / 2; pi / 2]; 27 | % Tf = 3; 28 | % N= 1000; 29 | % method = 5 ; 30 | % traj = JointTrajectory(thetastart, thetaend, Tf, N, method); 31 | % thetamat = traj; 32 | % dthetamat = zeros(1000, 3); 33 | % ddthetamat = zeros(1000, 3); 34 | % dt = Tf / (N - 1); 35 | % for i = 1: N - 1 36 | % dthetamat(i + 1, :) = (thetamat(i + 1, :) - thetamat(i, :)) / dt; 37 | % ddthetamat(i + 1, :) = (dthetamat(i + 1, :) - dthetamat(i, :)) / dt; 38 | % end 39 | % %Initialise robot descripstion (Example with 3 links) 40 | % g = [0; 0; -9.8]; 41 | % Ftipmat = ones(N, 6); 42 | % M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; 43 | % M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; 44 | % M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; 45 | % M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; 46 | % G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); 47 | % G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); 48 | % G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); 49 | % Glist = cat(3, G1, G2, G3); 50 | % Mlist = cat(3, M01, M12, M23, M34); 51 | % Slist = [[1; 0; 1; 0; 1; 0], ... 52 | % [0; 1; 0; -0.089; 0; 0], ... 53 | % [0; 1; 0; -0.089; 0; 0.425]]; 54 | % taumat = InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, ... 55 | % g, Ftipmat, Mlist, Glist, Slist); 56 | % %Output using matplotlib to plot the joint forces/torques 57 | % time=0: dt: Tf; 58 | % plot(time, taumat(:, 1), 'b') 59 | % hold on 60 | % plot(time, taumat(:, 2), 'g') 61 | % plot(time, taumat(:, 3), 'r') 62 | % title('Plot for Torque Trajectories') 63 | % xlabel('Time') 64 | % ylabel('Torque') 65 | % legend('Tau1', 'Tau2', 'Tau3') 66 | % 67 | 68 | thetamat = thetamat'; 69 | dthetamat = dthetamat'; 70 | ddthetamat = ddthetamat'; 71 | Ftipmat = Ftipmat'; 72 | taumat = thetamat; 73 | for i = 1: size(thetamat, 2) 74 | taumat(:, i) ... 75 | = InverseDynamics(thetamat(:, i), dthetamat(:, i), ddthetamat(:, i), ... 76 | g, Ftipmat(:, i), Mlist, Glist, Slist); 77 | end 78 | taumat = taumat'; 79 | end -------------------------------------------------------------------------------- /contat_lib/slblocks.m: -------------------------------------------------------------------------------- 1 | function blkStruct = slblocks 2 | %SLBLOCKS Defines the block library for a specific Toolbox or Blockset. 3 | % SLBLOCKS returns information about a Blockset to Simulink. The 4 | % information returned is in the form of a BlocksetStruct with the 5 | % following fields: 6 | % 7 | % Name Name of the Blockset in the Simulink block library 8 | % Blocksets & Toolboxes subsystem. 9 | % OpenFcn MATLAB expression (function) to call when you 10 | % double-click on the block in the Blocksets & Toolboxes 11 | % subsystem. 12 | % MaskDisplay Optional field that specifies the Mask Display commands 13 | % to use for the block in the Blocksets & Toolboxes 14 | % subsystem. 15 | % Browser Array of Simulink Library Browser structures, described 16 | % below. 17 | % 18 | % The Simulink Library Browser needs to know which libraries in your 19 | % Blockset it should show, and what names to give them. To provide 20 | % this information, define an array of Browser data structures with one 21 | % array element for each library to display in the Simulink Library 22 | % Browser. Each array element has two fields: 23 | % 24 | % Library File name of the library (model file) to include in the 25 | % Library Browser. 26 | % Name Name displayed for the library in the Library Browser 27 | % window. Note that the Name is not required to be the 28 | % same as the model file name. 29 | % 30 | % Example: 31 | % 32 | % % 33 | % % Define the BlocksetStruct for the Simulink block libraries 34 | % % Only simulink_extras shows up in Blocksets & Toolboxes 35 | % % 36 | % blkStruct.Name = ['Simulink' sprintf('\n') 'Extras']; 37 | % blkStruct.OpenFcn = 'simulink_extras'; 38 | % blkStruct.MaskDisplay = sprintf('Simulink\nExtras'); 39 | % 40 | % % 41 | % % Both simulink and simulink_extras show up in the Library Browser. 42 | % % 43 | % blkStruct.Browser(1).Library = 'simulink'; 44 | % blkStruct.Browser(1).Name = 'Simulink'; 45 | % blkStruct.Browser(2).Library = 'simulink_extras'; 46 | % blkStruct.Browser(2).Name = 'Simulink Extras'; 47 | % 48 | 49 | % Copyright 1990-2015 The MathWorks, Inc. 50 | 51 | % 52 | % Name of the subsystem which will show up in the Simulink Blocksets 53 | % and Toolboxes subsystem. 54 | % 55 | blkStruct.Name = ['SimMechanics Contact Forces Library']; 56 | % 57 | % The function that will be called when the user double-clicks on 58 | % this icon. 59 | % 60 | blkStruct.OpenFcn = 'Contact_Forces_Lib'; 61 | 62 | % 63 | % The argument to be set as the Mask Display for the subsystem. You 64 | % may comment this line out if no specific mask is desired. 65 | % Example: blkStruct.MaskDisplay = 'plot([0:2*pi],sin([0:2*pi]));'; 66 | % No display for Simulink Extras. 67 | % 68 | blkStruct.MaskDisplay = 'SimMechanics Contact Forces Library'; 69 | 70 | % 71 | % Define the Browser structure array, the first element contains the 72 | % information for the Simulink block library and the second for the 73 | % Simulink Extras block library. 74 | % 75 | 76 | %blkStruct.Browser(1).Library = 'simulink'; 77 | %blkStruct.Browser(1).Name = 'Simulink'; 78 | %blkStruct.Browser(2).Library = 'Contact_Forces_Lib'; 79 | %blkStruct.Browser(2).Name = 'SimMechanics Contact Forces Library'; 80 | 81 | %blkStruct.Browser(1).Library = 'Contact_Forces_Lib'; 82 | %blkStruct.Browser(1).Name = 'SimMechanics Contact Forces Library'; 83 | 84 | Browser(1).Library = 'Contact_Forces_Lib'; 85 | Browser(1).Name = 'SimMechanics Contact Forces Library'; 86 | 87 | %Browser(1).IsTopLevel = 0; 88 | %Browser(1).IsFlat = 1;% Is this library "flat" (i.e. no subsystems)? 89 | 90 | %Browser(2).Library = 'TNO_dtlib_2'; 91 | %Browser(2).Name = 'Simulink'; 92 | %Browser(2).IsFlat = 0;% Is this library "flat" (i.e. no subsystems)? 93 | 94 | blkStruct.Browser = Browser; 95 | clear Browser; 96 | 97 | 98 | % Define information for model updater 99 | blkStruct.ModelUpdaterMethods.fhDetermineBrokenLinks = @UpdateSimulinkBrokenLinksMappingHelper; 100 | blkStruct.ModelUpdaterMethods.fhSeparatedChecks = @UpdateSimulinkBlocksHelper; 101 | 102 | % End of slblocks 103 | 104 | 105 | -------------------------------------------------------------------------------- /mr/ForwardDynamicsTrajectory.m: -------------------------------------------------------------------------------- 1 | function [thetamat, dthetamat] ... 2 | = ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, ... 3 | Ftipmat, Mlist, Glist, Slist, dt, ... 4 | intRes) 5 | % *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** 6 | % Takes thetalist: n-vector of initial joint variables, 7 | % dthetalist: n-vector of initial joint rates, 8 | % taumat: An N x n matrix of joint forces/torques, where each row is 9 | % the joint effort at any time step, 10 | % g: Gravity vector g, 11 | % Ftipmat: An N x 6 matrix of spatial forces applied by the 12 | % end-effector (If there are no tip forces, the user should 13 | % input a zero and a zero matrix will be used), 14 | % Mlist: List of link frames {i} relative to {i-1} at the home 15 | % position, 16 | % Glist: Spatial inertia matrices Gi of the links, 17 | % Slist: Screw axes Si of the joints in a space frame, in the format 18 | % of a matrix with the screw axes as the columns, 19 | % dt: The timestep between consecutive joint forces/torques, 20 | % intRes: Integration resolution is the number of times integration 21 | % (Euler) takes places between each time step. Must be an 22 | % integer value greater than or equal to 1. 23 | % Returns thetamat: The N x n matrix of robot joint angles resulting from 24 | % the specified joint forces/torques, 25 | % dthetamat: The N x n matrix of robot joint velocities. 26 | % This function simulates the motion of a serial chain given an open-loop 27 | % history of joint forces/torques. It calls a numerical integration 28 | % procedure that uses ForwardDynamics. 29 | % Example Inputs (3 Link Robot): 30 | % 31 | % clc; clear; 32 | % thetalist = [0.1; 0.1; 0.1]; 33 | % dthetalist = [0.1; 0.2; 0.3]; 34 | % taumat = [[3.63, -6.58, -5.57]; [3.74, -5.55, -5.5]; ... 35 | % [4.31, -0.68, -5.19]; [5.18, 5.63, -4.31]; ... 36 | % [5.85, 8.17, -2.59]; [5.78, 2.79, -1.7]; ... 37 | % [4.99, -5.3, -1.19]; [4.08, -9.41, 0.07]; ... 38 | % [3.56, -10.1, 0.97]; [3.49, -9.41, 1.23]]; 39 | % %Initialise robot description (Example with 3 links) 40 | % g = [0; 0; -9.8]; 41 | % Ftipmat = ones(size(taumat, 1), 6); 42 | % M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; 43 | % M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; 44 | % M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; 45 | % M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; 46 | % G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); 47 | % G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); 48 | % G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); 49 | % Glist = cat(3, G1, G2, G3); 50 | % Mlist = cat(3, M01, M12, M23, M34); 51 | % Slist = [[1; 0; 1; 0; 1; 0], ... 52 | % [0; 1; 0; -0.089; 0; 0], ... 53 | % [0; 1; 0; -0.089; 0; 0.425]]; 54 | % dt = 0.1; 55 | % intRes = 8; 56 | % [thetamat, dthetamat] ... 57 | % = ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, ... 58 | % Ftipmat, Mlist, Glist, Slist, dt, intRes); 59 | % %Output using matplotlib to plot the joint forces/torques 60 | % Tf = size(taumat, 1); 61 | % time=0: (Tf / size(thetamat, 1)): (Tf - (Tf / size(thetamat, 1))); 62 | % plot(time,thetamat(:, 1),'b') 63 | % hold on 64 | % plot(time,thetamat(:, 2), 'g') 65 | % plot(time,thetamat(:, 3), 'r') 66 | % plot(time,dthetamat(:, 1), 'c') 67 | % plot(time,dthetamat(:, 2), 'm') 68 | % plot(time,dthetamat(:, 3), 'y') 69 | % title('Plot of Joint Angles and Joint Velocities') 70 | % xlabel('Time') 71 | % ylabel('Joint Angles/Velocities') 72 | % legend('Theta1', 'Theta2', 'Theta3', 'DTheta1', 'DTheta2', 'DTheta3') 73 | % 74 | 75 | taumat = taumat'; 76 | Ftipmat = Ftipmat'; 77 | thetamat = taumat; 78 | thetamat(:, 1) = thetalist; 79 | dthetamat = taumat; 80 | dthetamat(:, 1) = dthetalist; 81 | for i = 1: size(taumat, 2) - 1 82 | for j = 1: intRes 83 | ddthetalist ... 84 | = ForwardDynamics(thetalist, dthetalist, taumat(:,i), g, ... 85 | Ftipmat(:, i), Mlist, Glist, Slist); 86 | [thetalist, dthetalist] = EulerStep(thetalist, dthetalist, ... 87 | ddthetalist, dt / intRes); 88 | end 89 | thetamat(:, i + 1) = thetalist; 90 | dthetamat(:, i + 1) = dthetalist; 91 | end 92 | thetamat = thetamat'; 93 | dthetamat = dthetamat'; 94 | end -------------------------------------------------------------------------------- /init_quad_whole_body_ctrl.m: -------------------------------------------------------------------------------- 1 | % Jun-1-2020 2 | % this new model is used to study whole body dynamics control 3 | addpath(genpath(fullfile(pwd,'contat_lib'))); 4 | %parameters for ground plane geometry and contract 5 | ground.stiff = 5e4; 6 | ground.damp = 1000; 7 | ground.height = 0.4; 8 | ground.terrain_u = 0.7; 9 | 10 | %% a quad param, this is a standard format 11 | quad_param.leg_num = 4; 12 | %%robot body size 13 | quad_param.x_length = 0.6; 14 | quad_param.y_length = 0.3; 15 | quad_param.z_length = 0.15; 16 | quad_param.chassis_density = 600; 17 | quad_param.shoulder_size = 0.07; 18 | quad_param.shoulder_density = 660; 19 | quad_param.limb_width = 0.04; 20 | quad_param.upper_length = 0.16; 21 | quad_param.lower_length = 0.25; 22 | quad_param.limb_density = 660; 23 | quad_param.foot_radius = 0.035; % foot density is 0 24 | quad_param.shoulder_distance = 0.2; 25 | quad_param.max_stretch = quad_param.upper_length + quad_param.lower_length; 26 | quad_param.knee_damping = 0.1; 27 | %%robot mass and inertia 28 | quad_param.body_mass = quad_param.chassis_density*quad_param.x_length*quad_param.y_length*quad_param.z_length; 29 | quad_param.should_mass = quad_param.shoulder_density*quad_param.shoulder_size^3; 30 | quad_param.upper_leg_mass = quad_param.limb_density*quad_param.limb_width*quad_param.limb_width*quad_param.upper_length; 31 | quad_param.lower_leg_mass = quad_param.limb_density*quad_param.limb_width*quad_param.limb_width*quad_param.lower_length; 32 | 33 | quad_param.total_mass = quad_param.body_mass + 4*(quad_param.should_mass+... 34 | quad_param.upper_leg_mass+quad_param.lower_leg_mass); 35 | quad_param.g = 9.805; 36 | quad_param.leg_l1 = quad_param.upper_length; 37 | quad_param.leg_l2 = quad_param.lower_length+quad_param.foot_radius; 38 | 39 | %% leg mounts. Used to calculate R_cs t_cs 40 | dx = quad_param.x_length/2-0.1; 41 | dy = quad_param.y_length/2+quad_param.shoulder_size/2; 42 | quad_param.leg1_mount_x = dx; 43 | quad_param.leg1_mount_y = dy; 44 | quad_param.leg1_mount_angle = pi/2; 45 | quad_param.R_cs(:,:,1) = [cos(quad_param.leg1_mount_angle) -sin(quad_param.leg1_mount_angle) 0; 46 | sin(quad_param.leg1_mount_angle) cos(quad_param.leg1_mount_angle) 0; 47 | 0 0 1]; 48 | quad_param.t_cs(:,1) = [quad_param.leg1_mount_x; 49 | quad_param.leg1_mount_y; 50 | 0]; 51 | 52 | quad_param.leg2_mount_x = dx; 53 | quad_param.leg2_mount_y = -dy; 54 | quad_param.leg2_mount_angle = -pi/2; 55 | quad_param.R_cs(:,:,2) = [cos(quad_param.leg2_mount_angle) -sin(quad_param.leg2_mount_angle) 0; 56 | sin(quad_param.leg2_mount_angle) cos(quad_param.leg2_mount_angle) 0; 57 | 0 0 1]; 58 | quad_param.t_cs(:,2) = [quad_param.leg2_mount_x; 59 | quad_param.leg2_mount_y; 60 | 0]; 61 | 62 | quad_param.leg3_mount_x = -dx; 63 | quad_param.leg3_mount_y = dy; 64 | quad_param.leg3_mount_angle = pi/2; 65 | quad_param.R_cs(:,:,3) = [cos(quad_param.leg3_mount_angle) -sin(quad_param.leg3_mount_angle) 0; 66 | sin(quad_param.leg3_mount_angle) cos(quad_param.leg3_mount_angle) 0; 67 | 0 0 1]; 68 | quad_param.t_cs(:,3) = [quad_param.leg3_mount_x; 69 | quad_param.leg3_mount_y; 70 | 0]; 71 | 72 | quad_param.leg4_mount_x = -dx; 73 | quad_param.leg4_mount_y = -dy; 74 | quad_param.leg4_mount_angle = -pi/2; 75 | quad_param.R_cs(:,:,4) = [cos(quad_param.leg4_mount_angle) -sin(quad_param.leg4_mount_angle) 0; 76 | sin(quad_param.leg4_mount_angle) cos(quad_param.leg4_mount_angle) 0; 77 | 0 0 1]; 78 | quad_param.t_cs(:,4) = [quad_param.leg4_mount_x; 79 | quad_param.leg4_mount_y; 80 | 0]; 81 | %% joint twists 82 | quad_param.leg_joint1_w = [0;0;1]; 83 | quad_param.leg_joint2_w = [0;1;0]; 84 | quad_param.leg_joint3_w = [0;1;0]; 85 | quad_param.leg_joint1_body_q = [-(quad_param.leg_l1+quad_param.leg_l2);0;0]; 86 | quad_param.leg_joint2_body_q = [-(quad_param.leg_l1+quad_param.leg_l2);0;0]; 87 | quad_param.leg_joint3_body_q = [-(quad_param.leg_l2);0;0]; 88 | quad_param.leg_joint1_spatial_q = [0;0;0]; 89 | quad_param.leg_joint2_spatial_q = [0;0;0]; 90 | quad_param.leg_joint3_spatial_q = [quad_param.leg_l1;0;0]; 91 | quad_param.Slist_link1 = [[quad_param.leg_joint1_w;-cross(quad_param.leg_joint1_w,quad_param.leg_joint1_spatial_q)], ... 92 | [quad_param.leg_joint2_w;-cross(quad_param.leg_joint2_w,quad_param.leg_joint2_spatial_q)]]; 93 | 94 | quad_param.Slist_link2 = [[quad_param.leg_joint1_w;-cross(quad_param.leg_joint1_w,quad_param.leg_joint1_spatial_q)], ... 95 | [quad_param.leg_joint2_w;-cross(quad_param.leg_joint2_w,quad_param.leg_joint2_spatial_q)], ... 96 | [quad_param.leg_joint3_w;-cross(quad_param.leg_joint3_w,quad_param.leg_joint3_spatial_q)]]; 97 | quad_param.Blist_link2 = [[quad_param.leg_joint1_w;-cross(quad_param.leg_joint1_w,quad_param.leg_joint1_body_q)], ... 98 | [quad_param.leg_joint2_w;-cross(quad_param.leg_joint2_w,quad_param.leg_joint2_body_q)], ... 99 | [quad_param.leg_joint3_w;-cross(quad_param.leg_joint3_w,quad_param.leg_joint3_body_q)]]; 100 | 101 | 102 | %% home positions, these configurations does not include offsets. They are 103 | % just used for visualization 104 | quad_param.M1 = [[1, 0, 0, quad_param.leg_l1/2]; % home configuration of link1 105 | [0, 1, 0, 0]; 106 | [0, 0, 1, 0]; 107 | [0, 0, 0, 1]]; 108 | quad_param.M2 = [[1, 0, 0, quad_param.leg_l1+quad_param.leg_l2/2]; % home configuration of link2 109 | [0, 1, 0, 0]; 110 | [0, 0, 1, 0]; 111 | [0, 0, 0, 1]]; 112 | quad_param.M3 = [[1, 0, 0, quad_param.leg_l1+quad_param.leg_l2]; % home configuration of link2 113 | [0, 1, 0, 0]; 114 | [0, 0, 1, 0]; 115 | [0, 0, 0, 1]]; 116 | 117 | quad_param.home_ang = [0;0;pi/2]; 118 | % parameters for leg control 119 | ctrl.pos_kp = 30; 120 | ctrl.pos_ki = 0; 121 | ctrl.pos_kd = 0; 122 | ctrl.vel_kp = 4.4; 123 | ctrl.vel_ki = 0; 124 | ctrl.vel_kd = 0.0; 125 | 126 | 127 | 128 | 129 | -------------------------------------------------------------------------------- /mr/SimulateControl.m: -------------------------------------------------------------------------------- 1 | function [taumat, thetamat] ... 2 | = SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, ... 3 | Glist, Slist, thetamatd, dthetamatd, ... 4 | ddthetamatd, gtilde, Mtildelist, Gtildelist, ... 5 | Kp, Ki, Kd, dt, intRes) 6 | % *** CHAPTER 11: ROBOT CONTROL *** 7 | % Takes thetalist: n-vector of initial joint variables, 8 | % dthetalist: n-vector of initial joint velocities, 9 | % g: Actual gravity vector g, 10 | % Ftipmat: An N x 6 matrix of spatial forces applied by the 11 | % end-effector (If there are no tip forces, the user should 12 | % input a zero and a zero matrix will be used), 13 | % Mlist: Actual list of link frames i relative to i? at the home 14 | % position, 15 | % Glist: Actual spatial inertia matrices Gi of the links, 16 | % Slist: Screw axes Si of the joints in a space frame, in the format 17 | % of a matrix with the screw axes as the columns, 18 | % thetamatd: An Nxn matrix of desired joint variables from the 19 | % reference trajectory, 20 | % dthetamatd: An Nxn matrix of desired joint velocities, 21 | % ddthetamatd: An Nxn matrix of desired joint accelerations, 22 | % gtilde: The gravity vector based on the model of the actual robot 23 | % (actual values given above), 24 | % Mtildelist: The link frame locations based on the model of the 25 | % actual robot (actual values given above), 26 | % Gtildelist: The link spatial inertias based on the model of the 27 | % actual robot (actual values given above), 28 | % Kp: The feedback proportional gain (identical for each joint), 29 | % Ki: The feedback integral gain (identical for each joint), 30 | % Kd: The feedback derivative gain (identical for each joint), 31 | % dt: The timestep between points on the reference trajectory. 32 | % intRes: Integration resolution is the number of times integration 33 | % (Euler) takes places between each time step. Must be an 34 | % integer value greater than or equal to 1. 35 | % Returns taumat: An Nxn matrix of the controller commanded joint 36 | % forces/torques, where each row of n forces/torques 37 | % corresponds to a single time instant, 38 | % thetamat: An Nxn matrix of actual joint angles. 39 | % The end of this function plots all the actual and desired joint angles. 40 | % Example Usage 41 | % 42 | % clc; clear; 43 | % thetalist = [0.1; 0.1; 0.1]; 44 | % dthetalist = [0.1; 0.2; 0.3]; 45 | % %Initialize robot description (Example with 3 links) 46 | % g = [0; 0; -9.8]; 47 | % M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]]; 48 | % M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; 49 | % M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]]; 50 | % M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]]; 51 | % G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]); 52 | % G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]); 53 | % G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]); 54 | % Glist = cat(3, G1, G2, G3); 55 | % Mlist = cat(3, M01, M12, M23, M34); 56 | % Slist = [[1; 0; 1; 0; 1; 0], ... 57 | % [0; 1; 0; -0.089; 0; 0], ... 58 | % [0; 1; 0; -0.089; 0; 0.425]]; 59 | % dt = 0.01; 60 | % %Create a trajectory to follow 61 | % thetaend =[pi / 2; pi; 1.5 * pi]; 62 | % Tf = 1; 63 | % N = Tf / dt; 64 | % method = 5; 65 | % thetamatd = JointTrajectory(thetalist, thetaend, Tf, N, method); 66 | % dthetamatd = zeros(N, 3); 67 | % ddthetamatd = zeros(N, 3); 68 | % dt = Tf / (N - 1); 69 | % for i = 1: N - 1 70 | % dthetamatd(i + 1, :) = (thetamatd(i + 1, :) - thetamatd(i, :)) / dt; 71 | % ddthetamatd(i + 1, :) = (dthetamatd(i + 1, :) ... 72 | % - dthetamatd(i, :)) / dt; 73 | % end 74 | % %Possibly wrong robot description (Example with 3 links) 75 | % gtilde = [0.8; 0.2; -8.8]; 76 | % Mhat01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.1]; [0, 0, 0, 1]]; 77 | % Mhat12 = [[0, 0, 1, 0.3]; [0, 1, 0, 0.2]; [-1, 0 ,0, 0]; [0, 0, 0, 1]]; 78 | % Mhat23 = [[1, 0, 0, 0]; [0, 1, 0, -0.2]; [0, 0, 1, 0.4]; [0, 0, 0, 1]]; 79 | % Mhat34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.2]; [0, 0, 0, 1]]; 80 | % Ghat1 = diag([0.1, 0.1, 0.1, 4, 4, 4]); 81 | % Ghat2 = diag([0.3, 0.3, 0.1, 9, 9, 9]); 82 | % Ghat3 = diag([0.1, 0.1, 0.1, 3, 3, 3]); 83 | % Gtildelist = cat(3, Ghat1, Ghat2, Ghat3); 84 | % Mtildelist = cat(4, Mhat01, Mhat12, Mhat23, Mhat34); 85 | % Ftipmat = ones(N, 6); 86 | % Kp = 20; 87 | % Ki = 10; 88 | % Kd = 18; 89 | % intRes = 8; 90 | % [taumat, thetamat] ... 91 | % = SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, ... 92 | % Slist, thetamatd, dthetamatd, ddthetamatd, gtilde, ... 93 | % Mtildelist, Gtildelist, Kp, Ki, Kd, dt, intRes); 94 | % 95 | 96 | Ftipmat = Ftipmat'; 97 | thetamatd = thetamatd'; 98 | dthetamatd = dthetamatd'; 99 | ddthetamatd = ddthetamatd'; 100 | n = size(thetamatd, 2); 101 | taumat = zeros(size(thetamatd)); 102 | thetamat = zeros(size(thetamatd)); 103 | thetacurrent = thetalist; 104 | dthetacurrent = dthetalist; 105 | eint = zeros(size(thetamatd, 1), 1); 106 | for i=1: n 107 | taulist ... 108 | = ComputedTorque(thetacurrent, dthetacurrent, eint, gtilde, ... 109 | Mtildelist, Gtildelist, Slist, thetamatd(:, i), ... 110 | dthetamatd(:, i), ddthetamatd(:, i), Kp, Ki, Kd); 111 | for j=1: intRes 112 | ddthetalist ... 113 | = ForwardDynamics(thetacurrent, dthetacurrent, taulist, g, ... 114 | Ftipmat(:, i), Mlist, Glist, Slist); 115 | [thetacurrent, dthetacurrent] ... 116 | = EulerStep(thetacurrent, dthetacurrent, ddthetalist, ... 117 | (dt / intRes)); 118 | end 119 | taumat(:, i) = taulist; 120 | thetamat(:, i) = thetacurrent; 121 | eint = eint + (dt*(thetamatd(:, i) - thetacurrent)); 122 | end 123 | %Output using matplotlib 124 | links = size(thetamat, 1); 125 | leg = cell(1, 2 * links); 126 | time=0: dt: dt * n - dt; 127 | timed=0: dt: dt * n - dt; 128 | figure 129 | hold on 130 | for i=1: links 131 | col = rand(1, 3); 132 | plot(time, (thetamat(i, :)'), '-', 'Color', col) 133 | plot(timed, (thetamatd(i, :)'), '.', 'Color', col) 134 | leg{2 * i - 1} = (strcat('ActualTheta', num2str(i))); 135 | leg{2 * i} = (strcat('DesiredTheta', num2str(i))); 136 | end 137 | title('Plot of Actual and Desired Joint Angles') 138 | xlabel('Time') 139 | ylabel('Joint Angles') 140 | legend(leg, 'Location', 'NorthWest') 141 | taumat = taumat'; 142 | thetamat = thetamat'; 143 | end --------------------------------------------------------------------------------