Main Content

Esta página es para la versión anterior. La página correspondiente en inglés ha sido eliminada en la versión actual.

Planificar una trayectoria de alcance con múltiples restricciones cinemáticas

En este ejemplo se muestra cómo utilizar la cinemática inversa generalizada para planificar una trayectoria de espacio conjunto para un manipulador robótico. Combina múltiples restricciones para generar una trayectoria que guía la pinza a una taza que descansa sobre una mesa. Estas restricciones garantizan que la pinza se acerca a la copa en línea recta y que la pinza permanece a una distancia segura de la mesa, sin necesidad de que las posturas de la pinza se determinen de antemano.

Configurar el modelo de robot

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

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

Defina las dimensiones de la copa.

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

Agregue un cuerpo fijo al modelo robot que representa el centro de la copa.

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 de robot que cumplan los siguientes criterios:

  • Comience en la configuración del hogar

  • Sin cambios bruscos en la configuración del robot

  • Mantenga la pinza por lo menos 5 cm por encima de la "mesa" (z a 0)

  • La pinza debe estar alineada con la copa a medida que se acerca

  • Terminar con la pinza a 5 cm del centro de la copa

En este ejemplo se utilizan objetos de restricción para generar configuraciones de robot que cumplen estos criterios. La trayectoria generada consta de cinco waypoints de configuración. El primer waypoint, , se establece como la configuración de inicio.q0 Preasignar el resto de las configuraciones en el uso de .qWaypointsrepmat

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

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

  • Límites cartesianos - Limita la altura de la pinza

  • Un destino de posición: especifica la posición de la copa en relación con la pinza.

  • Una restricción de apuntamiento - Alinea la pinza con el eje de la copa

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

  • Límites de posición de unión: limita el cambio en las posiciones de unión entre waypoints.

gik = generalizedInverseKinematics('RigidBodyTree', lbr, ...     'ConstraintInputs', {'cartesian','position','aiming','orientation','joint'})
gik =    generalizedInverseKinematics with properties:        NumConstraints: 5     ConstraintInputs: {1x5 cell}        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 solucionador. Estos objetos contienen los parámetros necesarios para cada restricción. Modifique estos parámetros entre las llamadas al solucionador 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 tabla (dirección z negativa). Todos los demás valores se dan como o .inf-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 de la copa en relación con 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 sea aproximadamente vertical, colocando el objetivo muy por encima del robot.iiwa_link_ee El marco está orientado de tal manera que esta restricción alinea la pinza con el eje de la copa.iiwa_link_ee

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 unión. Establezca la propiedad de esta restricción en función de la configuración anterior para limitar el cambio en las posiciones de unión.Bounds

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 del pinzamiento coincida con el valor especificado por la propiedad.TargetOrientation Utilice esta restricción para fijar la orientación de la pinza durante el enfoque final de la copa.

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 en la Copa

Esta configuración debe colocar la pinza a una distancia de la copa, de modo que el enfoque final se pueda hacer con la pinza correctamente alineada.

intermediateDistance = 0.3;

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

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

Establezca la posición de destino de la copa en el marco de la pinza. La copa debe estar en el eje z de la pinza a la distancia especificada.

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

Resuelva para la configuración del robot que satisface las restricciones de entrada utilizando el solucionador.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 mueven a la pinza a la copa a lo largo de una línea recta

Vuelva a habilitar las restricciones de orientación y límite de posición de unión.

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

Deshabilite la restricción align-with-cup, ya que la restricción de orientación la hace redundante.

alignWithCup.Weights = 0;

Establezca la restricción de orientación para que contendrá la orientación en función de la configuración anterior ( ).qWaypoints(2,:) Obtenga la transformación de la pinza a la base del modelo de robot. Convierte la transformación homogénea en un cuaternión.

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

Defina la distancia entre la copa 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 unión entre cada waypoint.

maxJointChange = deg2rad(10);

Llame al solucionador 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

Interpolar entre los waypoints para generar una trayectoria suave. Se utiliza para evitar los excesos, que podrían violar los límites de las articulaciones del robot.pchip

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

Calcular la posición del pinzamiento 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 la taza

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

Animar el manipulador y trazar 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

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

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