matlab Robotics System Toolbox 解析解

4月16！

于 2022-06-15 10:05:41 发布

阅读量406

点赞数 1

分类专栏：机器人文章标签： matlab

本文链接：https://blog.csdn.net/qq_35007834/article/details/125291622

版权

机器人专栏收录该内容

9 篇文章 0 订阅

订阅专栏

analyticalInverseKinematics

clear;clc;
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("MaxNumBodies",6,"DataFormat",'row');

body1 = rigidBody('body1');
jnt1 = rigidBodyJoint('jnt1','revolute');
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(jnt1,dhparams(1,:),'dh');
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');

body1.Joint = jnt1;
body2.Joint = jnt2;
body3.Joint = jnt3;
body4.Joint = jnt4;
body5.Joint = jnt5;
body6.Joint = jnt6;

addBody(robot,body1,'base')
addBody(robot,body2,'body1')
addBody(robot,body3,'body2')
addBody(robot,body4,'body3')
addBody(robot,body5,'body4')
addBody(robot,body6,'body5')

showdetails(robot)

analyticalik = analyticalInverseKinematics(robot);
analyticalik.KinematicGroup
 
eeBodyName = analyticalik.KinematicGroup.EndEffectorBodyName;
baseName = analyticalik.KinematicGroup.BaseName;
showdetails(analyticalik)
generateIKFunction(analyticalik,'robotIK');

expJoint = [pi/5,pi/3,pi/2,-pi/7,pi/4,pi/6]
expEEPose = getTransform(robot,expJoint,eeBodyName,baseName);

ikConfig = robotIK(expEEPose,true)