Main Content

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 una trayectoria de alcance con múltiples restricciones cinemáticas

Abrir script en vivo

Este ejemplo muestra cómo utilizar cinemática inversa generalizada para planificar una trayectoria de espacio articular para un manipulador robótico. Combina múltiples restricciones para generar una trayectoria que guía la pinza hasta un vaso que descansa sobre una mesa. Estas limitaciones garantizan que la pinza se acerque al vaso en línea recta y permanezca a una distancia segura de la mesa, sin que sea necesario determinar de antemano las poses de la pinza.

Configurar el modelo de robot

Este ejemplo usa un modelo del KUKA LBR iiwa, un robot manipulador con 7 grados de libertad. importrobot genera un modelo rigidBodyTree a partir de una descripción almacenada en un archivo URDF (formato de descripción de robot unificado).

lbr = importrobot('iiwa14.urdf'); % 14 kg payload version
lbr.DataFormat = 'row';
gripper = 'iiwa_link_ee_kuka';

Defina las dimensiones del vaso.

cupHeight = 0.2;
cupRadius = 0.05;
cupPosition = [-0.5, 0.5, cupHeight/2];

Añada un cuerpo fijo al modelo de robot que represente el centro del vaso.

body = rigidBody('cupFrame');
setFixedTransform(body.Joint, trvec2tform(cupPosition))
addBody(lbr, body, lbr.BaseName);

Definir el problema de planificación

El objetivo de este ejemplo es generar una secuencia de configuraciones del robot que satisfagan los siguientes criterios:

  • Comenzar en la configuración inicial

  • Ningún cambio abrupto en la configuración del robot

  • Mantener la pinza al menos 5 cm por encima de la "mesa" (z = 0)

  • La pinza debe estar alineada con el vaso a medida que se acerca

  • Terminar con la pinza a 5 cm del centro del vaso

Este ejemplo usa objetos de restricción para generar configuraciones del robot que satisfagan estos criterios. La trayectoria generada consta de cinco waypoints de configuración. El primer waypoint, q0, se establece como la configuración inicial. Preasigne el resto de las configuraciones en qWaypoints usando repmat.

numWaypoints = 5;
q0 = homeConfiguration(lbr);
qWaypoints = repmat(q0, numWaypoints, 1);

Cree un solver generalizedInverseKinematics que acepte las siguientes entradas de restricción:

  • Límites cartesianos: limitan la altura de la pinza

  • Un objetivo de posición: especifica la posición del vaso respecto a la pinza

  • Una restricción de apuntamiento: alinea la pinza con el eje del vaso

  • Un objetivo de orientación: mantiene una orientación fija para la pinza mientras se acerca al vaso

  • Límites de posición de las articulaciones: limitan el cambio en las posiciones de las articulaciones entre waypoints

gik = generalizedInverseKinematics('RigidBodyTree', lbr, ...
    'ConstraintInputs', {'cartesian','position','aiming','orientation','joint'})
gik = 
  generalizedInverseKinematics with properties:

      NumConstraints: 5
    ConstraintInputs: {'cartesian'  'position'  'aiming'  'orientation'  'joint'}
       RigidBodyTree: [1x1 rigidBodyTree]
     SolverAlgorithm: 'BFGSGradientProjection'
    SolverParameters: [1x1 struct]

Crear objetos de restricción

Cree los objetos de restricción que se pasan como entradas al solver. Esos objetos contienen los parámetros necesarios para cada restricción. Modifique esos parámetros entre llamadas al solver según sea necesario.

Cree una restricción de límites cartesianos que requiera que la pinza esté al menos 5 cm por encima de la mesa (dirección z negativa). Todos los demás valores se proporcionan como inf o -inf.

heightAboveTable = constraintCartesianBounds(gripper);
heightAboveTable.Bounds = [-inf, inf; ...
                           -inf, inf; ...
                           0.05, inf]
heightAboveTable = 
  constraintCartesianBounds with properties:

        EndEffector: 'iiwa_link_ee_kuka'
      ReferenceBody: ''
    TargetTransform: [4x4 double]
             Bounds: [3x2 double]
            Weights: [1 1 1]

Cree una restricción en la posición del vaso respecto a la pinza, con una tolerancia de 5 mm.

distanceFromCup = constraintPositionTarget('cupFrame');
distanceFromCup.ReferenceBody = gripper;
distanceFromCup.PositionTolerance = 0.005
distanceFromCup = 
  constraintPositionTarget with properties:

          EndEffector: 'cupFrame'
        ReferenceBody: 'iiwa_link_ee_kuka'
       TargetPosition: [0 0 0]
    PositionTolerance: 0.0050
              Weights: 1

Cree una restricción de apuntamiento que requiera que el eje z del marco iiwa_link_ee sea aproximadamente vertical, colocando el objetivo muy por encima del robot. El marco iiwa_link_ee está orientado de tal manera que esta restricción alinea la pinza con el eje del vaso.

alignWithCup = constraintAiming('iiwa_link_ee');
alignWithCup.TargetPoint = [0, 0, 100]
alignWithCup = 
  constraintAiming with properties:

         EndEffector: 'iiwa_link_ee'
       ReferenceBody: ''
         TargetPoint: [0 0 100]
    AngularTolerance: 0
             Weights: 1

Cree una restricción de límites de posición de la articulación. Establezca la propiedad Bounds de esta restricción en función de la configuración anterior para limitar el cambio en las posiciones de la articulación.

limitJointChange = constraintJointBounds(lbr)
limitJointChange = 
  constraintJointBounds with properties:

     Bounds: [7x2 double]
    Weights: [1 1 1 1 1 1 1]

Cree una restricción de orientación para la pinza con una tolerancia de un grado. Esta restricción requiere que la orientación de la pinza coincida con el valor especificado por la propiedad TargetOrientation. Utilice esta restricción para fijar la orientación de la pinza durante el acercamiento final al vaso.

fixOrientation = constraintOrientationTarget(gripper);
fixOrientation.OrientationTolerance = deg2rad(1)
fixOrientation = 
  constraintOrientationTarget with properties:

             EndEffector: 'iiwa_link_ee_kuka'
           ReferenceBody: ''
       TargetOrientation: [1 0 0 0]
    OrientationTolerance: 0.0175
                 Weights: 1

Encontrar una configuración que apunte al vaso

Esta configuración debe colocar la pinza a una distancia del vaso, de modo que la aproximación final se pueda realizar con la pinza correctamente alineada.

intermediateDistance = 0.3;

Los objetos de restricción tienen una propiedad Weights que determina cómo trata el solver las restricciones en conflicto. Establecer las ponderaciones de una restricción en cero desactiva la restricción. Para esta configuración, desactive los límites de posición de la articulación y la restricción de orientación.

limitJointChange.Weights = zeros(size(limitJointChange.Weights));
fixOrientation.Weights = 0;

Establezca la posición de destino para el vaso en el marco de la pinza. El vaso debe descansar sobre el eje z de la pinza a la distancia especificada.

distanceFromCup.TargetPosition = [0,0,intermediateDistance];

Resuelva la configuración del robot que satisface las restricciones de entrada usando el solver gik. Debe especificar todas las restricciones de entrada. Establezca esa configuración como el segundo waypoint.

[qWaypoints(2,:),solutionInfo] = gik(q0, heightAboveTable, ...
                       distanceFromCup, alignWithCup, fixOrientation, ...
                       limitJointChange);

Buscar configuraciones que muevan la pinza hasta el vaso a lo largo de una línea recta

Vuelva a activar el límite de posición de la articulación y las restricciones de orientación.

limitJointChange.Weights = ones(size(limitJointChange.Weights));
fixOrientation.Weights = 1;

Desactive la restricción de alineación con el vaso, ya que la restricción de orientación la hace redundante.

alignWithCup.Weights = 0;

Establezca la restricción de orientación para mantener la orientación en función de la configuración anterior (qWaypoints(2,:)). Obtenga la transformación desde la pinza hasta la base del modelo de robot. Convierta la transformación homogénea en un cuaternión.

fixOrientation.TargetOrientation = ...
    tform2quat(getTransform(lbr,qWaypoints(2,:),gripper));

Defina la distancia entre el vaso y la pinza para cada waypoint.

finalDistanceFromCup = 0.05;
distanceFromCupValues = linspace(intermediateDistance, finalDistanceFromCup, numWaypoints-1);

Defina el cambio máximo permitido en las posiciones de las articulaciones entre cada waypoint.

maxJointChange = deg2rad(10);

Llame al solver para cada waypoint restante.

for k = 3:numWaypoints
    % Update the target position.
    distanceFromCup.TargetPosition(3) = distanceFromCupValues(k-1);
    % Restrict the joint positions to lie close to their previous values.
    limitJointChange.Bounds = [qWaypoints(k-1,:)' - maxJointChange, ...
                               qWaypoints(k-1,:)' + maxJointChange];
    % Solve for a configuration and add it to the waypoints array.
    [qWaypoints(k,:),solutionInfo] = gik(qWaypoints(k-1,:), ...
                                         heightAboveTable, ...
                                         distanceFromCup, alignWithCup, ...
                                         fixOrientation, limitJointChange);
end

Visualizar la trayectoria generada

Interpole entre los waypoints para generar una trayectoria sin obstáculos. Use pchip para evitar sobreimpulsos, que podrían infringir los límites de las articulaciones del robot.

framerate = 15;
r = rateControl(framerate);
tFinal = 10;
tWaypoints = [0,linspace(tFinal/2,tFinal,size(qWaypoints,1)-1)];
numFrames = tFinal*framerate;
qInterp = pchip(tWaypoints,qWaypoints',linspace(0,tFinal,numFrames))';

Calcule la posición de la pinza para cada configuración interpolada.

gripperPosition = zeros(numFrames,3);
for k = 1:numFrames
    gripperPosition(k,:) = tform2trvec(getTransform(lbr,qInterp(k,:), ...
                                                    gripper));
end

Muestre el robot en su configuración inicial junto con la mesa y el vaso.

figure;
show(lbr, qWaypoints(1,:), 'PreservePlot', false);
hold on
exampleHelperPlotCupAndTable(cupHeight, cupRadius, cupPosition);
p = plot3(gripperPosition(1,1), gripperPosition(1,2), gripperPosition(1,3));

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 34 objects of type patch, line. These objects represent world, iiwa_link_0, iiwa_link_1, iiwa_link_2, iiwa_link_3, iiwa_link_4, iiwa_link_5, iiwa_link_6, iiwa_link_7, iiwa_link_ee, iiwa_link_ee_kuka, cupFrame, iiwa_link_0_mesh, iiwa_link_1_mesh, iiwa_link_2_mesh, iiwa_link_3_mesh, iiwa_link_4_mesh, iiwa_link_5_mesh, iiwa_link_6_mesh, iiwa_link_7_mesh.

Anime el manipulador y represente la posición de la pinza.

hold on
for k = 1:size(qInterp,1)
    show(lbr, qInterp(k,:), 'PreservePlot', false);
    p.XData(k) = gripperPosition(k,1);
    p.YData(k) = gripperPosition(k,2);
    p.ZData(k) = gripperPosition(k,3);
    waitfor(r);
end
hold off

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 34 objects of type patch, line. These objects represent world, iiwa_link_0, iiwa_link_1, iiwa_link_2, iiwa_link_3, iiwa_link_4, iiwa_link_5, iiwa_link_6, iiwa_link_7, iiwa_link_ee, iiwa_link_ee_kuka, cupFrame, iiwa_link_0_mesh, iiwa_link_1_mesh, iiwa_link_2_mesh, iiwa_link_3_mesh, iiwa_link_4_mesh, iiwa_link_5_mesh, iiwa_link_6_mesh, iiwa_link_7_mesh.

Si desea guardar las configuraciones generadas en un archivo MAT para su uso posterior, ejecute lo siguiente:

>> save('lbr_trajectory.mat', 'tWaypoints', 'qWaypoints');