clear all; clc;
dh=[0 60 0 pi/2;
0 0 -315 0;
0 0 -310 0 ;
0 0 -75 -pi/2];
body1 = rigidBody('body1');
jnt1 = rigidBodyJoint('jnt1','revolute');
jnt1.HomePosition = 0;
tform = dh2homogene(dh(1,:));
setFixedTransform(jnt1,tform);
body1.Joint = jnt1;
robot = rigidBodyTree;
addBody(robot,body1,'base')
body2 = rigidBody('body2');
jnt2 = rigidBodyJoint('jnt2','revolute');
jnt2.HomePosition = pi/6;
tform2 = dh2homogene(dh(2,:));
setFixedTransform(jnt2,tform2);
body2.Joint = jnt2;
addBody(robot,body2,'body1');
body3 = rigidBody('body3');
body4 = rigidBody('body4');
jnt3 = rigidBodyJoint('jnt3','revolute');
jnt4 = rigidBodyJoint('jnt4','revolute');
tform3 = dh2homogene(dh(3,:));
tform4 = dh2homogene(dh(4,:));
setFixedTransform(jnt3,tform3);
setFixedTransform(jnt4,tform4);
jnt3.HomePosition = 0;
body3.Joint = jnt3;
body4.Joint = jnt4;
addBody(robot,body3,'body2');
addBody(robot,body4,'body3');
bodyEndEffector = rigidBody('endeffector');
tform5 = trvec2tform([0, 0, 0]);
setFixedTransform(bodyEndEffector.Joint,tform5);
addBody(robot,bodyEndEffector,'body4');
config = homeConfiguration(robot);
show(robot)
0 Comments
Sign in to comment.