При создании симуляции движения робота kuka в Matlab выдается ошибка: Error using trapveltraj

Рейтинг: 1Ответов: 1Опубликовано: 05.07.2023

Нет опыта работы в 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

Ответы

▲ 0

Вы в ошибочной строке при использовании функции repmat поставили вторым элементом число 6, а надо 1. Потом у вас внезапно появляется переменная или функция joint_num, которая ранее не определена.