-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathlimberoGrieel_leg_manipulability.asv
110 lines (81 loc) · 3.45 KB
/
limberoGrieel_leg_manipulability.asv
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
clear
close all
clc
% Load limbero model (DH, DH contact, URDF) in the workspace
load('limbero_data');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% ELLIPSOID MANIPULABILITY %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% LIMBERO SWING %%
% Define useful varaibles:
N_link = limbero.n;
q0 = zeros(1, N_link);
q1 = [0 -pi*2/3 pi*2/3 -pi/4 0 0 0];
q = q0;
%% Compute manipulability from jacobian matrix
J = limbero.jacob0(q);
% Core of velocity manipulability ellipsoid
E = J*J';
Et = E(1:3, 1:3);
Er = E(4:6, 4:6);
% check if decoupling of rotation/translation can be numerically meaningful
coupling_ratio = norm(E(1:3, 4:6))/(norm(Et)+ norm(Er));
% plot robot and ellipsoid to check if meaningful
figure('Name', 'LIMBERO LF LEG,SWINGING(Coxa-Gripper), MANIPULABILITY');
limbero.plot(q, 'workspace', [-1 1 -1 1 -1 1], 'view', [30 30], 'scale', 0.6,'jaxes', 'nobase', 'noshadow', 'notiles');
title('Limbero+Grieel LF limb DH, swinging (q = qz), Manipulability');
% (roto)transalte to tool frame
T0 = limbero.fkine(q);
Tool_translation = T0.t;
Tool_rotation = T0.R;
% rescale for visualization purpose
%new_Et = Tool_rotation*Et*Tool_rotation'*0.005^2;
%new_Et = Et*0.005^2;
% plot ellipsoid
hold on
plot_ellipse(Et,Tool_translation, 'edgecolor', 'r');
%% Direct computation through roboticstoolbox:
figure('Name', 'LIMBERO LF LEG,SWINGING(Coxa-Gripper), MANIPULABILITY');
limbero.plot(q, 'workspace', [-1 1 -1 1 -1 1], 'view', [30 30], 'scale', 0.6,'jaxes', 'nobase', 'noshadow', 'notiles');
title('Limbero+Grieel LF limb DH, swinging (q = qz), Manipulability');
% plot ellipsoid
limbero.vellipse(q, 'fillcolor', 'b', 'edgecolor', 'w', 'alpha', 0.5);
pause()
%-----------------------------%%
%% LIMBERO CONTACT %%
close all
q0_contact = zeros(1,N_link); %configuration when in contact
%% Compute manipulability from jacobian matrix
J_contact = limbero_contact.jacob0(q_contact);
% Core of velocity manipulability ellipsoid
E_contact = J_contact*J_contact';
Et_contact = E_contact(1:3, 1:3);
Er_contact = E_ocntact(4:6, 4:6);
% check if decoupling of rotation/translation can be numerically meaningful
coupling_ratio_contact = norm(E_contact(1:3, 4:6))/(norm(Et_contact)+ norm(Er_contact));
% plot the robot and then ellipsoid
figure('Name', 'LIMBERO LF LEG,CONTACT(Gripper-Coxa), MANIPULABILITY');
limbero_contact.plot(q_contact, 'workspace', [-1 1 -1 1 -1 1], 'view', [30 30], 'scale', 0.6,'jaxes', 'nobase', 'noshadow', 'notiles');
title('Limbero+Grieel LF limb DH, contact (q = qz), Manipulability');
% revert z and y axis for a proper visualization
set(gca, 'Zdir', 'reverse');
set(gca, 'Ydir', 'reverse');
% (roto)transalte to tool frame
T0_contact = limbero_contact.fkine(q_contact);
Tool_translation_contact = T0.t;
Tool_rotation_contact = T0.R;
% rescale for visualization purpose
%new_Et = Tool_rotation*Et*Tool_rotation'*0.005^2;
%new_Et = Et*0.005^2;
% plot ellipsoid
hold on
plot_ellipse(Et_contact,Tool_translation_contact, 'edgecolor', 'r');
%% Direct computation through roboticstoolbox:
figure('Name', 'LIMBERO LF LEG,CONTACT(Gripper-Coxa), MANIPULABILITY');
limbero_contact.plot(q_contact, 'workspace', [-1 1 -1 1 -1 1], 'view', [30 30], 'scale', 0.6,'jaxes', 'nobase', 'noshadow', 'notiles');
title('Limbero+Grieel LF limb DH, contact (q = qz), Manipulability');
% revert z and y axis for a proper visualization
set(gca, 'Zdir', 'reverse');
set(gca, 'Ydir', 'reverse');
% plot ellipsoid
% limbero_contact.vellipse(q, 'fillcolor', 'b', 'edgecolor', 'w', 'alpha', 0.5);