python-tests/DQ_Kinematics_test.m at master · dqrobotics/python-tests · GitHub

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
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
clear all;
close all;
include_namespace_dq

NUMBER_OF_RANDOM = 1000;

serial_manipulator = KukaLwr4Robot.kinematics();
whole_body = KukaYoubot.kinematics();

%% Create a serial-whole-body robot
serial_whole_body = DQ_SerialWholeBody(serial_manipulator);
serial_whole_body.set_reference_frame(1 + 0.5*E_*(0.5*k_));
serial_whole_body.add(FrankaEmikaPandaRobot.kinematics());

%% Generate data for unary and binary operators
random_q = random('unif',-pi,pi,[7 NUMBER_OF_RANDOM]);
random_q_dot = random('unif',-pi,pi,[7 NUMBER_OF_RANDOM]);
random_q_whole_body = random('unif',-pi,pi,[8 NUMBER_OF_RANDOM]);
random_q_serial_whole_body = random('unif',-pi,pi,[serial_whole_body.get_dim_configuration_space() NUMBER_OF_RANDOM]);
random_dq_a = random('unif',-10,10,[8 NUMBER_OF_RANDOM]);
%random_dq_b = random('unif',-10,10,[8 NUMBER_OF_RANDOM]);

%% Pre-allocate results
result_of_fkm = zeros(8, NUMBER_OF_RANDOM);
result_of_pose_jacobian = zeros(8, 7, NUMBER_OF_RANDOM);
result_of_pose_jacobian_derivative = zeros(8, 7, NUMBER_OF_RANDOM);

result_of_whole_body_fkm = zeros(8, NUMBER_OF_RANDOM);
result_of_whole_body_jacobian = zeros(8, 8, NUMBER_OF_RANDOM);

result_of_serial_whole_body_fkm      = zeros(8, NUMBER_OF_RANDOM);
result_of_serial_whole_body_raw_fkm  = zeros(8, NUMBER_OF_RANDOM);
result_of_serial_whole_body_jacobian = zeros(8, serial_whole_body.get_dim_configuration_space(), NUMBER_OF_RANDOM);

%       distance_jacobian - Compute the (squared) distance Jacobian.
result_of_distance_jacobian = zeros(1, 7, NUMBER_OF_RANDOM);
%       rotation_jacobian - Compute the rotation Jacobian.
result_of_rotation_jacobian = zeros(4, 7, NUMBER_OF_RANDOM);
%       translation_jacobian - Compute the translation Jacobian.
result_of_translation_jacobian = zeros(4, 7, NUMBER_OF_RANDOM);
%       line_jacobian - Compute the line Jacobian.
result_of_line_jacobian = zeros(8, 7, NUMBER_OF_RANDOM);
%       plane_jacobian - Compute the plane Jacobian.
result_of_plane_jacobian = zeros(8, 7, NUMBER_OF_RANDOM);


%       line_to_line_distance_jacobian  - Compute the line-to-line distance Jacobian.
result_of_line_to_line_distance_jacobian = zeros(1, 7, NUMBER_OF_RANDOM);
%       line_to_line_residual - Compute the line-to-line residual.

%       line_to_point_distance_jacobian - Compute the line-to-line distance Jacobian.
result_of_line_to_point_distance_jacobian = zeros(1, 7, NUMBER_OF_RANDOM);
%       line_to_point_residual - Compute the line-to-point residual.

%       plane_to_point_distance_jacobian - Compute the plane-to-point distance Jacobian.
result_of_plane_to_point_distance_jacobian = zeros(1, 7, NUMBER_OF_RANDOM);
%       plane_to_point_residual - Compute the plane-to-point residual.

%       point_to_line_distance_jacobian - Compute the point-to-line distance Jacobian.
result_of_point_to_line_distance_jacobian = zeros(1, 7, NUMBER_OF_RANDOM);
%       point_to_line_residual - Compute the point to line residual.

%       point_to_plane_distance_jacobian - Compute the point to plane distance Jacobian.
result_of_point_to_plane_distance_jacobian = zeros(1, 7, NUMBER_OF_RANDOM);
%       point_to_plane_residual - Compute the point to plane residual.


%       point_to_point_distance_jacobian - Compute the point to point distance Jacobian.
result_of_point_to_point_distance_jacobian = zeros(1, 7, NUMBER_OF_RANDOM);
%       point_to_point_residual - Compute the point to point residual.

%       line_to_line_angle_jacobian - Compute the line-to-line angle Jacobian.
result_of_line_to_line_angle_jacobian = zeros(1, 7, NUMBER_OF_RANDOM);


%% Loop
for i=1:NUMBER_OF_RANDOM
    %% Preliminaries
    ha = DQ(random_dq_a(:,i));

    %% DQ_SerialManipulator
    x_pose = serial_manipulator.fkm(random_q(:,i));
    J_pose = serial_manipulator.pose_jacobian(random_q(:,i));

    %% Preliminaries for DQ_Kinematics
    robot_point = translation(x_pose);
    robot_line = get_line_from_dq(x_pose, k_);
    robot_plane = get_plane_from_dq(x_pose, k_);
    workspace_line = get_line_from_dq(ha, k_);
    workspace_point = translation(normalize(ha));
    workspace_plane = get_plane_from_dq(ha, k_);
    translation_jacobian = DQ_Kinematics.translation_jacobian(J_pose, x_pose);
    line_jacobian = DQ_Kinematics.line_jacobian(J_pose, x_pose, k_);
    plane_jacobian = DQ_Kinematics.plane_jacobian(J_pose, x_pose, k_);

    %% Results of DQ_SerialManipulator
    result_of_fkm(:, i) = vec8(x_pose);
    result_of_pose_jacobian(:,:,i) = J_pose;
    result_of_pose_jacobian_derivative(:,:,i) = serial_manipulator.pose_jacobian_derivative(random_q(:,i),random_q_dot(:,i));
    %% Results of DQ_SerialWholeBody
    result_of_serial_whole_body_fkm(:, i)       = vec8(serial_whole_body.fkm(random_q_serial_whole_body(:,i)));
    result_of_serial_whole_body_raw_fkm(:, i)   = vec8(serial_whole_body.raw_fkm(random_q_serial_whole_body(:,i)));
    result_of_serial_whole_body_jacobian(:,:,i) = serial_whole_body.pose_jacobian(random_q_serial_whole_body(:,i));
    %% Results of DQ_WholeBody
    result_of_whole_body_fkm(:, i) = vec8(whole_body.fkm(random_q_whole_body(:,i)));
    result_of_whole_body_jacobian(:,:,i) = whole_body.pose_jacobian(random_q_whole_body(:,i));
    %% Results of DQ_Kinematics
    result_of_distance_jacobian(:,:,i) = DQ_Kinematics.distance_jacobian(J_pose,x_pose);
    result_of_rotation_jacobian(:,:,i) = DQ_Kinematics.rotation_jacobian(J_pose);
    result_of_translation_jacobian(:,:,i) = translation_jacobian;
    result_of_line_jacobian(:,:,i) = line_jacobian;
    result_of_plane_jacobian(:,:,i) = plane_jacobian;
    result_of_line_to_line_distance_jacobian(:,:,i) = DQ_Kinematics.line_to_line_distance_jacobian(line_jacobian,robot_line,workspace_line);
    result_of_line_to_point_distance_jacobian(:,:,i) = DQ_Kinematics.line_to_point_distance_jacobian(line_jacobian,robot_line,workspace_point);
    result_of_plane_to_point_distance_jacobian(:,:,i) = DQ_Kinematics.plane_to_point_distance_jacobian(plane_jacobian,workspace_point);
    result_of_point_to_line_distance_jacobian(:,:,i) = DQ_Kinematics.point_to_line_distance_jacobian(translation_jacobian,robot_point,workspace_line);
    result_of_point_to_plane_distance_jacobian(:,:,i) = DQ_Kinematics.point_to_plane_distance_jacobian(translation_jacobian,robot_point,workspace_plane);
    result_of_point_to_point_distance_jacobian(:,:,i) = DQ_Kinematics.point_to_point_distance_jacobian(translation_jacobian,robot_point,workspace_point);
    result_of_line_to_line_angle_jacobian(:,:,i) = DQ_Kinematics.line_to_line_angle_jacobian(line_jacobian,robot_line,workspace_line);
end

save DQ_Kinematics_test.mat

function ret = get_line_from_dq(dq, primitive)
include_namespace_dq
dq = normalize(dq);
ret = Ad(rotation(dq), primitive) + E_*cross(translation(dq), Ad(rotation(dq), primitive));
end

function ret = get_plane_from_dq(dq, primitive)
include_namespace_dq
dq = normalize(dq);
ret = Ad(rotation(dq), primitive) + E_*dot(translation(dq), Ad(rotation(dq), primitive));
end