При создании симуляции движения робота kuka в Matlab выдается ошибка: Error using trapveltraj
Нет опыта работы в Matlab. Пытаюсь сделать визуализацию движения робота-манипулятора. Известны координаты точек, по которым должен двигаться рука робота (обратная кинематика). Сделала код по примерам, выдается ошибка:
**Error using trapveltraj (line 104)**
The dimensions of EndTime do not match the provided Waypoints matrix. For an NxP Waypoints matrix,
the Name-Value pair entries must be either a scalar, an Nx1 vector, or an Nx(P-1) matrix.
Можете посмотреть и указать возможные причины возникновения данной ошибки?
**Error in untitled30 (line 110)**
[q,qd,qdd] = trapveltraj(jointWaypoints,numel(trajTimes), ...
clear
close all
clc
dhparams = [0 0.975 0.26 (-pi/2);
0 0 0.680 0 ;
(-pi/2) 0 -0.035 (-pi/2);
0 0.670 0 pi/2 ;
0 0 0 (-pi/2);
0 0.115 0 0 ];
robot = rigidBodyTree;
body1 = rigidBody('body1');
body2 = rigidBody('body2');
body3 = rigidBody('body3');
body4 = rigidBody('body4');
body5 = rigidBody('body5');
body6 = rigidBody('body6');
joint1 = rigidBodyJoint('joint1', 'revolute');
joint2 = rigidBodyJoint('joint2', 'revolute');
joint3 = rigidBodyJoint('joint3', 'revolute');
joint4 = rigidBodyJoint('joint4', 'revolute');
joint5 = rigidBodyJoint('joint5', 'revolute');
joint6 = rigidBodyJoint('joint6', 'revolute');
setFixedTransform(joint1, dhparams(1,:), "dh");
setFixedTransform(joint2, dhparams(2,:), "dh");
setFixedTransform(joint3, dhparams(3,:), "dh");
setFixedTransform(joint4, dhparams(4,:), "dh");
setFixedTransform(joint5, dhparams(5,:), "dh");
setFixedTransform(joint6, dhparams(6,:), "dh");
body1.Joint = joint1;
body2.Joint = joint2;
body3.Joint = joint3;
body4.Joint = joint4;
body5.Joint = joint5;
body6.Joint = joint6;
addBody(robot, body1, 'base')
addBody(robot, body2, 'body1')
addBody(robot, body3, 'body2')
addBody(robot, body4, 'body3')
addBody(robot, body5, 'body4')
addBody(robot, body6, 'body5')
%show(robot);
%axis([-100,100,-100,100,-100,100])
%axis off
randConfig = robot.randomConfiguration;
tform = getTransform(robot, randConfig, 'body6',
'base');
show(robot, randConfig);
%create iK solver for robot
ik = inverseKinematics('RigidBodyTree', robot);
weights = [1 1 1 1 1 1];
initialguess = robot.homeConfiguration;
%find joint configurations for specified end-
effector pose incl. weights
%i.e. tolerances
[configSoln, solInfo] = ik('body6', tform, weights,
initialguess);
%show(robot, configSoln);
toolPositionHome = [-33.8969193 -78.9970322
98.2957916];
waypoints = toolPositionHome' + ...
[1214.55933 1427.87708 776.315063;
1210.57593 1429.51245 604.782410;
1216.69104 1429.17371 594.248169;
1222.99170 1429.13208 582.369141;
1225.72498 1428.44519 571.774353;
1227.22485 1429.49780 554.312439]';
orientations = [-2.21756506 40.4588852
-92.9536591;
-2.24066806 40.9752350 -92.9837952;
-2.68645883 50.9661140 -93.6014633;
-4.01838255 65.0880203 -95.1596451;
-6.12927 73.9510574 -97.4061890;
-29.2634926 86.5380630 -120.732971]';
waypointTimes = 0:15:30;
ts = 1;
trajTimes = 0:ts:waypointTimes(end);
waypointVels = 0.1 *[ 0 1 0;
-1 0 0;
0 -1 0;
1 0 0;
0 1 0
1 0 0]';
waypointAccels = zeros(size(waypointVels));
waypointAccelTimes = diff(waypointTimes)/4;
ikWeights = [1 1 1 1 1 1];
ikInitGuess = initialguess';
plotMode = 1; % 0 = None, 1 = Trajectory, 2 =
Coordinate Frames
show(robot,
initialguess','Frames','off','PreservePlot',false);
hold on
if plotMode == 1
hTraj =
plot3(waypoints(1,1),waypoints(2,1),
waypoints(3,1),'b.-');
end
plot3(waypoints(1,:),waypoints(2,:),
waypoints(3,:),'ro','LineWidth',2);
axis auto;
view([30 15]);
includeOrientation = true;
numWaypoints = size(waypoints,1);
numJoints = numel(robot.homeConfiguration);
jointWaypoints = zeros(numJoints,numWaypoints);
for idx = 1:numWaypoints
if includeOrientation
tgtPose = trvec2tform(waypoints(:,idx)') *
eul2tform(orientations(:,idx)');
else
tgtPose = trvec2tform(waypoints(:,idx)') *
eul2tform([pi/2 0 pi/2]); %#ok<UNRCH>
end
[config, info] = ik('body6', tgtPose, ikWeights,
ikInitGuess');
jointWaypoints(:,idx) = config.JointPosition;
end
trajType = 'trap';
switch trajType
case 'trap'
[q,qd,qdd] =
trapveltraj(jointWaypoints,numel(trajTimes), ...
'AccelTime',repmat(waypointAccelTimes,
[numJoints 6]), ...
'EndTime',repmat(diff(waypointTimes),
[numJoints 6]));
case 'cubic'
[q,qd,qdd] =
cubicpolytraj(jointWaypoints,waypointTimes,
trajTimes, ...
'VelocityBoundaryCondition',zeros(numJoints,numWaypoints));
case 'quintic'
[q,qd,qdd] = quinticpolytraj(jointWaypoints,waypointTimes,trajTimes, ...
'VelocityBoundaryCondition',zeros(numJoints,numWaypoints), ...
'AccelerationBoundaryCondition',zeros(numJoints,numWaypoints));
case 'bspline'
ctrlpoints = jointWaypoints; % Can adapt this as
needed
[q,qd,qdd] =
bsplinepolytraj(ctrlpoints,waypointTimes([1
end]),trajTimes);
otherwise
error('Invalid trajectory type! Use ''trap'',
''cubic'', ''quintic'', or ''bspline''');
end
for idx = 1:numel(trajTimes)
config(joint_num).JointPosition =
q(joint_num, idx)';
% Find Cartesian points for visualization
eeTform = getTransform(robot,config,'body6');
if plotMode == 1
eePos = tform2trvec(eeTform);
set(hTraj,'xdata',[hTraj.XData eePos(1)], ...
'ydata',[hTraj.YData eePos(2)], ...
'zdata',[hTraj.ZData eePos(3)]);
elseif plotMode == 2
plotTransforms(tform2trvec(eeTform),tform2quat(eeTform),'FrameSize',1);
end
jointWaypoints(:,idx) = arrayfun(@(x)
x.JointPosition, config)';
% Show the robot
show(robot,config','Frames','off','PreservePlot',false);
title(['Trajectory at t = '
num2str(trajTimes(idx))])
drawnow
end
Источник: Stack Overflow на русском