Matlab/my_rigid_body.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
body1=rigidBody('body1');
jnt1=rigidBodyJoint('jnt1','revolute');
jnt1.HomePosition=pi/4;
tform=trvec2tform([0.25, 0.25, 0]);
setFixedTransform(jnt1,tform);
body1.Joint=jnt1;
robot=rigidBodyTree;
addVisual(body1,'Cylinder',[0.03,0.3]);
addBody(robot,body1,'base');
body2=rigidBody('body2');
jnt2=rigidBodyJoint('jnt2','revolute');
jnt2.HomePosition=pi/6;
tform2=trvec2tform([1,0,0]);
setFixedTransform(jnt2,tform2);
addVisual(body2,'Cylinder',[0.03,0.3]);
body2.Joint=jnt2;
addBody(robot,body2,'body1');
body3=rigidBody('body3');
jnt3=rigidBodyJoint('jnt3','revolute');
jnt3.HomePosition=pi/4;
tform3=trvec2tform([0.6, -0.1, 0])*eul2tform([-pi/2, 0, 0]);
setFixedTransform(jnt3,tform3);
body3.Joint=jnt3;
addVisual(body3,'Cylinder',[0.03 0.3]);
body4=rigidBody('body4');
jnt4=rigidBodyJoint('jnt4','revolute');
%jnt4.HomePosition=pi/6;
tform4=trvec2tform([1, 0, 0]);
setFixedTransform(jnt4,tform4);
addVisual(body4,'Cylinder',[0.03 0.3]);
body4.Joint=jnt4;
body5=rigidBody('body5');
jnt5=rigidBodyJoint('jnt5','revolute');
jnt5.HomePosition=pi/4;
tform5=trvec2tform([1, 0.7, 0]);
setFixedTransform(jnt5,tform5);
addVisual(body5,'Cylinder',[0.03 0.3]);
body5.Joint=jnt5;
addBody(robot,body3,'body2');
addBody(robot,body4,'body3');
addBody(robot,body5,'body3');
config=homeConfiguration(robot);
tform=getTransform(robot,config,'body5','base');
%config(1).JointPosition=pi/2;
disp(tform);
disp(config);
show(robot,config);
showdetails(robot);