В этом примере показано, как сгенерировать и симулировать интерполированные объединенные траектории, чтобы переместиться от начальной буквы до желаемого положения исполнительного элемента конца. Синхронизация траекторий основана на аппроксимированной желаемой скорости конца инструмента руки (EOAT).
Загрузите модель робота дерева твердого тела (RBT) КИНОВОЙ Gen3.
robot = loadrobot('kinovaGen3','DataFormat','row','Gravity',[0 0 -9.81]);
Установите текущую настройку соединения робота.
currentRobotJConfig = homeConfiguration(robot);
Получите количество соединений и исполнительного элемента конца система координат RBT.
numJoints = numel(currentRobotJConfig);
endEffector = "EndEffector_Link";
Задайте временной шаг траектории и аппроксимируйте желаемую скорость инструмента.
timeStep = 0.1; % seconds toolSpeed = 0.1; % m/s
Установите начальное и итоговое положение исполнительного элемента конца.
jointInit = currentRobotJConfig; taskInit = getTransform(robot,jointInit,endEffector); taskFinal = trvec2tform([0.4,0,0.6])*axang2tform([0 1 0 pi]);
Вычислите пространственную траекторию задачи waypoints через интерполяцию.
Во-первых, вычислите расстояние путешествующего инструмента.
distance = norm(tform2trvec(taskInit)-tform2trvec(taskFinal));
Затем задайте времена траектории на основе путешествования на расстояние и желаемую скорость инструмента.
initTime = 0; finalTime = (distance/toolSpeed) - initTime; trajTimes = initTime:timeStep:finalTime; timeInterval = [trajTimes(1); trajTimes(end)];
Интерполируйте между taskInit
и taskFinal
вычислить промежуточный пробел задачи waypoints.
[taskWaypoints,taskVelocities] = transformtraj(taskInit,taskFinal,timeInterval,trajTimes);
Создайте объединенную модель движения пробела для управления PD на соединениях. taskSpaceMotionModel
объектные модели движение модели дерева твердого тела под управлением пропорциональной производной пробела задачи.
tsMotionModel = taskSpaceMotionModel('RigidBodyTree',robot,'EndEffectorName','EndEffector_Link');
Установите пропорциональные и производные усиления на ориентации обнулять, так, чтобы управляемое поведение только следовало за ссылочными положениями:
tsMotionModel.Kp(1:3,1:3) = 0; tsMotionModel.Kd(1:3,1:3) = 0;
Задайте начальные состояния (объединенные положения и скорости).
q0 = currentRobotJConfig; qd0 = zeros(size(q0));
Используйте ode15s
симулировать движение робота. Для этой проблемы система с обратной связью жестка, означая, что существует различие в масштабировании где-нибудь в проблеме. В результате интегратор иногда обеспечивается, чтобы сделать чрезвычайно небольшие шаги и нежесткий решатель ОДУ, такие как ode45
возьмет намного дольше, чтобы решить ту же задачу. Для получения дополнительной информации относитесь, чтобы Выбрать ODE Solver и Solve Stiff ODEs в документации.
Начиная со ссылочных изменений состояния в каждый момент функция обертки требуется, чтобы обновлять интерполированный вход траектории к производной состояния в каждый момент. Поэтому функция помощника в качестве примера передается как указатель на функцию решателю ОДУ. Результирующие состояния манипулятора выводятся в stateTask
.
[tTask,stateTask] = ode15s(@(t,state) exampleHelperTimeBasedTaskInputs(tsMotionModel,timeInterval,taskInit,taskFinal,t,state),timeInterval,[q0; qd0]);
Создайте объект инверсной кинематики для робота.
ik = inverseKinematics('RigidBodyTree',robot);
ik.SolverParameters.AllowRandomRestart = false;
weights = [1 1 1 1 1 1];
Вычислите начальные и желаемые объединенные настройки с помощью инверсной кинематики. Перенесите значения к пи, чтобы гарантировать, что интерполяция по минимальной области.
initialGuess = jointInit; jointFinal = ik(endEffector,taskFinal,weights,initialGuess);
По умолчанию решение IK уважает объединенные пределы. Однако для непрерывных соединений (шарнирные соединения с бесконечной областью значений), результирующие значения могут быть излишне большими и могут быть перенесены к [-pi, pi]
гарантировать, что итоговая траектория преодолевает минимальную дистанцию. Поскольку ненепрерывные соединения для Gen3 уже имеют пределы в этом интервале, достаточно просто перенести объединенные значения к pi
. Непрерывные соединения будут сопоставлены с интервалом [-pi, pi]
, и другие значения останутся то же самое.
wrappedJointFinal = wrapToPi(jointFinal);
Интерполируйте между ними использующий функцию кубического полинома, чтобы сгенерировать массив равномерно распределенных объединенных настроек. Используйте B-сплайн, чтобы сгенерировать сглаженную траекторию.
ctrlpoints = [jointInit',wrappedJointFinal']; jointConfigArray = cubicpolytraj(ctrlpoints,timeInterval,trajTimes); jointWaypoints = bsplinepolytraj(jointConfigArray,timeInterval,1);
Создайте объединенную модель движения пробела для управления PD на соединениях. jointSpaceMotionModel
объектные модели движение модели дерева твердого тела и управления пропорциональной производной использования на заданных объединенных положениях.
jsMotionModel = jointSpaceMotionModel('RigidBodyTree',robot,'MotionType','PDControl');
Установите начальные состояния (объединенные положения и скорости).
q0 = currentRobotJConfig; qd0 = zeros(size(q0));
Используйте ode15s
симулировать движение робота. Снова, функция помощника в качестве примера используется в качестве входа указателя на функцию к решателю ОДУ для того, чтобы обновить ссылочные входные параметры в каждый момент вовремя. Состояния объединенного пробела выводятся в stateJoint
.
[tJoint,stateJoint] = ode15s(@(t,state) exampleHelperTimeBasedJointInputs(jsMotionModel,timeInterval,jointConfigArray,t,state),timeInterval,[q0; qd0]);
Покажите начальную настройку робота.
show(robot,currentRobotJConfig,'PreservePlot',false,'Frames','off'); hold on axis([-1 1 -1 1 -0.1 1.5]);
Визуализируйте пространственную траекторию задачи. Выполните итерации через stateTask
состояния и интерполируют на основе текущего времени.
for i=1:length(trajTimes) % Current time tNow= trajTimes(i); % Interpolate simulated joint positions to get configuration at current time configNow = interp1(tTask,stateTask(:,1:numJoints),tNow); poseNow = getTransform(robot,configNow,endEffector); show(robot,configNow,'PreservePlot',false,'Frames','off'); taskSpaceMarker = plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'b.','MarkerSize',20); drawnow; end
Визуализируйте объединенную пространственную траекторию. Выполните итерации через jointTask
состояния и интерполируют на основе текущего времени.
% Return to initial configuration show(robot,currentRobotJConfig,'PreservePlot',false,'Frames','off'); for i=1:length(trajTimes) % Current time tNow= trajTimes(i); % Interpolate simulated joint positions to get configuration at current time configNow = interp1(tJoint,stateJoint(:,1:numJoints),tNow); poseNow = getTransform(robot,configNow,endEffector); show(robot,configNow,'PreservePlot',false,'Frames','off'); jointSpaceMarker = plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'r.','MarkerSize',20); drawnow; end % Add a legend and title legend([taskSpaceMarker jointSpaceMarker], {'Defined in Task-Space', 'Defined in Joint-Space'}); title('Manipulator Trajectories')