La traducción de esta página aún no se ha actualizado a la versión más reciente. Haga clic aquí para ver la última versión en inglés.

Planificar y ejecutar trayectorias de espacio cartesiano o articular utilizando el manipulador KINOVA Gen3

Abrir script en vivo

Este ejemplo muestra cómo generar y simular trayectorias de articulación interpoladas para pasar de una pose inicial a una pose deseada del efector final. La temporización de las trayectorias se basa en una velocidad deseada aproximada de la herramienta al final del brazo robótico (EOAT).

Cargue el modelo de robot de árbol de cuerpo rígido (RBT) de KINOVA Gen3.

robot = loadrobot('kinovaGen3','DataFormat','row','Gravity',[0 0 -9.81]);

Defina la configuración actual de la articulación del robot.

currentRobotJConfig = homeConfiguration(robot);

Obtenga el número de articulaciones y el marco de RBT del efector final.

numJoints = numel(currentRobotJConfig);
endEffector = "EndEffector_Link";

Especifique la unidad de tiempo de la trayectoria y la velocidad aproximada deseada de la herramienta.

timeStep = 0.1; % seconds
toolSpeed = 0.1; % m/s

Defina la pose inicial y final del efector final.

jointInit = currentRobotJConfig;
taskInit = getTransform(robot,jointInit,endEffector);

taskFinal = trvec2tform([0.4,0,0.6])*axang2tform([0 1 0 pi]);

Generar una trayectoria de espacio cartesiano

Calcule los waypoints de la trayectoria de espacio cartesiano mediante interpolación.

Primero, calcule la distancia de desplazamiento de la herramienta.

distance = norm(tform2trvec(taskInit)-tform2trvec(taskFinal));

Luego, defina los tiempos de trayectoria a partir de la distancia de desplazamiento y la velocidad deseada de la herramienta.

initTime = 0;
finalTime = (distance/toolSpeed) - initTime;
trajTimes = initTime:timeStep:finalTime;
timeInterval = [trajTimes(1); trajTimes(end)];

Interpole entre taskInit y taskFinal para calcular los waypoints intermedios de espacio cartesiano.

[taskWaypoints,taskVelocities] = transformtraj(taskInit,taskFinal,timeInterval,trajTimes);

Controlar el movimiento en el espacio cartesiano

Cree un modelo de movimiento en el espacio cartesiano para el control PD en las articulaciones. El objeto taskSpaceMotionModel modela el movimiento de un modelo de árbol de cuerpo rígido bajo un control proporcional derivativo del espacio cartesiano.

tsMotionModel = taskSpaceMotionModel('RigidBodyTree',robot,'EndEffectorName','EndEffector_Link');

Establezca en cero la ganancia proporcional y derivada en la orientación, de modo que el comportamiento controlado simplemente sigue las posiciones de referencia:

tsMotionModel.Kp(1:3,1:3) = 0;
tsMotionModel.Kd(1:3,1:3) = 0;

Defina los estados iniciales (posiciones de las articulaciones y velocidades).

q0 = currentRobotJConfig; 
qd0 = zeros(size(q0));

Utilice ode15s para simular el movimiento del robot. Para este problema, el sistema de lazo cerrado es rígido, lo que significa que existe una diferencia de escalado en algún punto del problema. Como resultado, el integrador a veces se ve obligado a realizar pasos sumamente pequeños, y un solver EDO no rígido como ode45 tardará mucho más tiempo en resolver el mismo problema. Para obtener más información, consulte Elegir un solver de ODE y Solve Stiff ODEs.

Dado que el estado de referencia cambia a cada instante, se necesita una función wrapper para actualizar la entrada de trayectoria interpolada en la derivada de estado en cada instante. Por tanto, se pasa una función helper de ejemplo como identificador de la función al solver EDO. Los estados resultantes del manipulador se muestran en stateTask.

[tTask,stateTask] = ode15s(@(t,state) exampleHelperTimeBasedTaskInputs(tsMotionModel,timeInterval,taskInit,taskFinal,t,state),timeInterval,[q0; qd0]);

Generar una trayectoria de espacio articular

Cree un objeto de cinemática inversa para el robot.

ik = inverseKinematics('RigidBodyTree',robot);
ik.SolverParameters.AllowRandomRestart = false;
weights = [1 1 1 1 1 1];

Calcule las configuraciones de articulaciones inicial y deseada utilizando cinemática inversa. Envuelva los valores en pi para asegurarse de que la interpolación supera un dominio mínimo.

initialGuess = jointInit;
jointFinal = ik(endEffector,taskFinal,weights,initialGuess);

De forma predeterminada, la solución IK respeta los límites de las articulaciones. Sin embargo, para las articulaciones continuas (articulaciones giratorias con rango infinito), los valores resultantes pueden ser innecesariamente grandes y se pueden envolver en [-pi, pi] para asegurar que la trayectoria final cubre una distancia mínima. Dado que las articulaciones no continuas para Gen3 ya tienen límites dentro de este intervalo, basta con envolver los valores de las articulaciones en pi. Las articulaciones continuas se asignarán al intervalo [-pi, pi] y los otros valores se mantendrán iguales.

wrappedJointFinal = wrapToPi(jointFinal);

Interpole entre ellos utilizando una función polinómica cúbica para generar un arreglo de configuraciones de las articulaciones repartidas uniformemente. Utilice una B-spline para generar una trayectoria sin obstáculos.

ctrlpoints = [jointInit',wrappedJointFinal'];
jointConfigArray = cubicpolytraj(ctrlpoints,timeInterval,trajTimes);
jointWaypoints = bsplinepolytraj(jointConfigArray,timeInterval,1);

Controlar una trayectoria de espacio articular

Cree un modelo de movimiento en el espacio articular para el control PD en las articulaciones. El objeto jointSpaceMotionModel modela el movimiento de un modelo de árbol de cuerpo rígido y utiliza el control proporcional derivativo en las posiciones especificadas de las articulaciones.

jsMotionModel = jointSpaceMotionModel('RigidBodyTree',robot,'MotionType','PDControl');

Establezca los estados iniciales (posiciones de las articulaciones y velocidades).

q0 = currentRobotJConfig; 
qd0 = zeros(size(q0));

Utilice ode15s para simular el movimiento del robot. De nuevo, se usa una función helper de ejemplo como entrada de identificador de la función en el solver EDO para actualizar las entradas de referencia en cada instante dado. Los estados del espacio articular se muestran en stateJoint.

[tJoint,stateJoint] = ode15s(@(t,state) exampleHelperTimeBasedJointInputs(jsMotionModel,timeInterval,jointConfigArray,t,state),timeInterval,[q0; qd0]);

Visualizar y comparar trayectorias de robot

Muestre la configuración inicial del robot.

show(robot,currentRobotJConfig,'PreservePlot',false,'Frames','off');
hold on
axis([-1 1 -1 1 -0.1 1.5]);

Visualice la trayectoria de espacio cartesiano. Itere a través de los estados de stateTask e interpole basándose en el momento actual.

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

Visualice la trayectoria de espacio articular. Itere a través de los estados de jointTask e interpole basándose en el momento actual.

% 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')