Matlab/loading_urdf.m at main · Dhanush-Poduval/Matlab · 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
robot=importrobot("./rudra_arm/urdf/rudra_arm_new.urdf");
robot.DataFormat="row";

showdetails(robot);
config=homeConfiguration(robot);

[isColliding, sepDist, witnessPts] = checkCollision(robot, config, ...
    "SkippedSelfCollisions", "adjacent");

disp(isColliding);
%{
disp(robot.BaseName);
cyllinder_radius=0.05;
final_position=[1.6,0.2,0.2];
scatter3(final_position(1),final_position(2),final_position(3),'yellow');
[x,y,z]=cylinder(cyllinder_radius);
x=x+1.6;
y=y+0.4;
z=z*0.4;
config = homeConfiguration(robot);
%check_base=getTransform(robot,config,"base_link");
%disp(check_base);
%q0=config;
%ik=inverseKinematics("RigidBodyTree",robot);
%endEffector="pulley";

%weights=[1 1 1 1 1 1];
%[a,~]=ik(endEffector,trvec2tform(final_position),weights,q0);
%disp(a);

%disp(config);
%}
%{
getTransform(robot,homeConfiguration(robot),"turntable","base_link")
figure;
ax=gca;
ax.Projection="orthographic";
hold on;
t = linspace(0, 1, 30);
qTrajectory = q0 + t' * (a-q0);
overall_frames=size(qTrajectory,1);
surf(x,y,z);
grid on;
view(3);
%}
%{
for i=1:overall_frames


        show(robot,qTrajectory(i,:),"PreservePlot",false,"FastUpdate",true,"Frames","off");


        current_eepos=getTransform(robot,qTrajectory(i,:),"turntable");
        current_ee=current_eepos(1:3,4);
        %{
        current_eepos2=getTransform(robot,qTrajectory(i,:),"roll");
        current_ee2=current_eepos2(1:3,4);
        current_eepos3=getTransform(robot,qTrajectory(i,:),"worm_pitch");
        current_ee3=current_eepos3(1:3,4);
        current_eepos4=getTransform(robot,qTrajectory(i,:),"pitch");
        current_ee4=current_eepos4(1:3,4);
        %}
        scatter3(current_ee(1),current_ee(2),current_ee(3),'filled','y');
        %{
        scatter3(current_ee2(1),current_ee2(2),current_ee2(3),'filled','r');
        scatter3(current_ee3(1),current_ee3(2),current_ee3(3),'black');
        scatter3(current_ee4(1),current_ee4(2),current_ee4(3),'green');
        %}
        disp(current_ee);
        %disp(qTrajectory(i,:));
        drawnow;

end
%}
%show(robot,a,"PreservePlot",true);
%camlight;
%lighting gouraud;
%material metal;
%{
for angle=-pi:0.1:pi
    i=1;
    disp(angle);
    config(5)=angle;
    link_config=getTransform(robot,config,'ypr pitch');
    flink_config=link_config(1:3,4);
    scatter3(flink_config(1),flink_config(2),flink_config(3),'filled','r');
    show(robot, config, "PreservePlot", false,'FastUpdate',true);
    drawnow;
end
%}
%{
joint=3;
valid=[];
for angle=-pi:0.02:pi
    config(joint)=angle;
    isColliding=checkCollision(robot,config,"SkippedSelfCollisions","parent");
    if ~isColliding
        valid(end+1)=angle;
    end

end
final_limits=[min(valid),max(valid)];
disp(final_limits);
%}
%{
for i = 1:numel(robot.Bodies)
    joint = robot.Bodies{i}.Joint;

    if joint.Type ~= "fixed"
        fprintf("Joint: %s\n", joint.Name);
        fprintf("Type: %s\n", joint.Type);
        fprintf("Limits: [%f , %f]\n", ...
            joint.PositionLimits(1), ...
            joint.PositionLimits(2));
        fprintf("\n");
    end
end
%}
%viztree=interactiveRigidBodyTree(robot,"MarkerBodyName","link1");
%showFigure(viztree);
%show(robot, config , "Frames","off", "PreservePlot", false);
%camorbit(90,0);