-
Notifications
You must be signed in to change notification settings - Fork 0
/
RBT.m
127 lines (119 loc) · 3.03 KB
/
RBT.m
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
% one
rbtree=rigidBodyTree;
body1=rigidBody('b1');
jnt1=rigidBodyJoint('jnt1','revolute');
body1.Joint=jnt1;
basename=rbtree.BaseName;
addBody(rbtree,body1,basename);
showdetails(rbtree)
% two
% first column is the distance between z axis(d), first column is angle
% rotate with x axis(\alpha); third is the distance between x axis (a)
% dhparams = [0 pi/2 0 0;
% 0.4318 0 0 0;
% 0.0203 -pi/2 0.15005 0;
% 0 pi/2 0.4318 0;
% 0 -pi/2 0 0;
% 0 0 0 0];
% robot=rigidBodyTree;
%
% body1 = rigidBody('body1');
% jnt1 = rigidBodyJoint('jnt1','revolute');
%
% setFixedTransform(jnt1,dhparams(1,:),'dh');
% body1.Joint = jnt1;
%
% addBody(robot,body1,'base')
%
% body2 = rigidBody('body2');
% jnt2 = rigidBodyJoint('jnt2','revolute');
% body3 = rigidBody('body3');
% jnt3 = rigidBodyJoint('jnt3','revolute');
% body4 = rigidBody('body4');
% jnt4 = rigidBodyJoint('jnt4','revolute');
% body5 = rigidBody('body5');
% jnt5 = rigidBodyJoint('jnt5','revolute');
% body6 = rigidBody('body6');
% jnt6 = rigidBodyJoint('jnt6','revolute');
%
% setFixedTransform(jnt2,dhparams(2,:),'dh');
% setFixedTransform(jnt3,dhparams(3,:),'dh');
% setFixedTransform(jnt4,dhparams(4,:),'dh');
% setFixedTransform(jnt5,dhparams(5,:),'dh');
% setFixedTransform(jnt6,dhparams(6,:),'dh');
%
% body2.Joint = jnt2;
% body3.Joint = jnt3;
% body4.Joint = jnt4;
% body5.Joint = jnt5;
% body6.Joint = jnt6;
%
% addBody(robot,body2,'body1')
% addBody(robot,body3,'body2')
% addBody(robot,body4,'body3')
% addBody(robot,body5,'body4')
% addBody(robot,body6,'body5')
%
% showdetails(robot)
%
% show(robot);
% axis([-0.5,0.5,-0.5,0.5,-0.5,0.5])
% axis on
% three
% load exampleRobots.mat
% % showdetails(puma1)
%
% body3=getBody(puma1,'L3');
% childBody=body3.Children{1};
%
% body3Copy = copy(body3);
% newJoint=rigidBodyJoint('prismatic');
% replaceJoint(puma1,'L3',newJoint);
%
% showdetails(puma1)
% % four
% robot=rigidBodyTree('DataFormat','row');
% body1 = rigidBody('body1');
% body2 = rigidBody('body2');
%
% joint1 = rigidBodyJoint('joint1','revolute');
% joint2 = rigidBodyJoint('joint2');
% setFixedTransform(joint2,trvec2tform([1 0 0]))
% body1.Joint = joint1;
% body2.Joint = joint2;
%
% body1.Mass = 2;
% body1.CenterOfMass = [0.5 0 0];
% body1.Inertia = [0.167 0.001 0.167 0 0 0];
%
% body2.Mass = 1;
% body2.CenterOfMass = [0 0 0];
% body2.Inertia = 0.0001*[4 4 4 0 0 0];
%
% addBody(robot,body1,'base');
% addBody(robot,body2,'body1');
%
% comPos = centerOfMass(robot);
%
% show(robot);
% hold on
% plot(comPos(1),comPos(2),'or')
% view(2)
%
% body2.Mass = 20;
% replaceBody(robot,'body2',body2)
%
% comPos2 = centerOfMass(robot);
% plot(comPos2(1),comPos2(2),'*g')
% hold off
% five
% load exampleRobots.mat lbr
% lbr.DataFormat = 'row';
% lbr.Gravity = [0 0 -9.81];% z axis direction
% q = homeConfiguration(lbr);
% wrench=[0 0 0.5 0 0 0.3];
% fext=externalForce(lbr,'tool0',wrench,q);
% qddot = forwardDynamics(lbr,q,[],[],fext);
% six
% robot = importrobot('iiwa14.urdf');
% show(robot);