Quadruped/Quad_Draw.m at master · sevenihust/Quadruped · 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
% drawing Quadrapeds robot
function [] = Quad_Draw(base_position, base_rotation, joints_angles)

%% calculate position and orientation of all joints

% @param joints_angles: 4x3 matrix,(num_legs, nums_joints)
quad_param;


joints_positions = zeros(3, max(nums_joints), num_legs);    % w.r.t. global frame
joints_rotations = zeros(3, 3, max(nums_joints), num_legs); % w.r.t. global frame

for i = 1:num_legs
    for j = 1:nums_joints(i)
        c_theta(j) = cos(joints_angles(i, j));
        s_theta(j) = sin(joints_angles(i, j));
    end
    joints_positions(:, 1, i) = base_rotation * p0(:, 1, i) + base_position;
    joints_rotations(:, :, 1, i) = base_rotation * R0(:, :, 1, i)...
        * [c_theta(1), -s_theta(1), 0; s_theta(1), c_theta(1), 0; 0, 0, 1];

    joints_positions(:, 2, i) = joints_rotations(:, :, 1, i)*p0(:, 2, i)...
        + joints_positions(:, 1, i);
    joints_rotations(:, :, 2, i) = joints_rotations(:, :, 1, i)*R0(:, :, 2, i)...
        * [c_theta(2), -s_theta(2), 0; s_theta(2), c_theta(2), 0; 0, 0, 1];

    joints_positions(:, 3, i) = joints_rotations(:, :, 2, i)*p0(:, 3, i)...
        + joints_positions(:, 2, i);
    joints_rotations(:, :, 3, i) = joints_rotations(:, :, 2, i)*R0(:, :, 3, i)...
        * [c_theta(3), -s_theta(3), 0; s_theta(3), c_theta(3), 0; 0, 0, 1];

    joints_positions(:, 4, i) = joints_rotations(:, :, 3, i)*p0(:, 4, i)...
        + joints_positions(:, 3, i);
%     joints_rotations(:, :, 4, i) = joints_rotations(:, :, 3, i)*R0(:, :, 4);
end
%% draw the robot
figure
view(3);
% draw global coordinate
% plot3(0, 0, 0, 'ro');
% draw_coordinate(zeros(3, 1), eye(3));
plot3(base_position(1), base_position(2), base_position(3), 'r+');
hold on
draw_coordinate(base_position, base_rotation);

%%% draw a box represent mobile base
line([joints_positions(1, 1, 1), joints_positions(1, 1, 2)],...
    [joints_positions(2, 1, 1), joints_positions(2, 1, 2)],...
    [joints_positions(3, 1, 1), joints_positions(3, 1, 2)],...
    'Color', 'c', 'LineWidth', 3.0);
line([joints_positions(1, 1, 1), joints_positions(1, 1, 3)],...
    [joints_positions(2, 1, 1), joints_positions(2, 1, 3)],...
    [joints_positions(3, 1, 1), joints_positions(3, 1, 3)],...
    'Color', 'c', 'LineWidth', 3.0);
line([joints_positions(1, 1, 2), joints_positions(1, 1, 4)],...
    [joints_positions(2, 1, 2), joints_positions(2, 1, 4)],...
    [joints_positions(3, 1, 2), joints_positions(3, 1, 4)],...
    'Color', 'c', 'LineWidth', 3.0);
line([joints_positions(1, 1, 3), joints_positions(1, 1, 4)],...
    [joints_positions(2, 1, 3), joints_positions(2, 1, 4)],...
    [joints_positions(3, 1, 3), joints_positions(3, 1, 4)],...
    'Color', 'c', 'LineWidth', 3.0);
line([base_position(1), joints_positions(1, 1, 3)],...
    [base_position(2), joints_positions(2, 1, 3)],...
    [base_position(3), joints_positions(3, 1, 3)],...
    'Color', 'c', 'LineWidth', 3.0);
line([base_position(1), joints_positions(1, 1, 4)],...
    [base_position(2), joints_positions(2, 1, 4)],...
    [base_position(3), joints_positions(3, 1, 4)],...
    'Color', 'c', 'LineWidth', 3.0);

for i = 1:num_legs
    line([joints_positions(1, 1, i), joints_positions(1, 2, i)],...
        [joints_positions(2, 1, i), joints_positions(2, 2, i)],...
        [joints_positions(3, 1, i), joints_positions(3, 2, i)],...
        'Color', 'c', 'LineWidth', 3.0);
    line([joints_positions(1, 2, i), joints_positions(1, 3, i)],...
        [joints_positions(2, 2, i), joints_positions(2, 3, i)],...
        [joints_positions(3, 2, i), joints_positions(3, 3, i)],...
        'Color', 'c', 'LineWidth', 3.0);
    line([joints_positions(1, 3, i), joints_positions(1, 4, i)],...
        [joints_positions(2, 3, i), joints_positions(2, 4, i)],...
        [joints_positions(3, 3, i), joints_positions(3, 4, i)],...
        'Color', 'c', 'LineWidth', 3.0);
    for j = 1:nums_joints(i)
        plot3(joints_positions(1, j, i), joints_positions(2, j, i), joints_positions(3, j, i));
        draw_coordinate(joints_positions(:, j, i), joints_rotations(:, :, j, i));
    end
end
grid on
% axis tight
axis equal
rotate3d on