Esta página aún no se ha traducido para esta versión. Puede ver la versión más reciente de esta página en inglés.

Planifique una trayectoria de alcance con varias restricciones cinemáticas

Este ejemplo muestra cómo utilizar la cinemática inversa generalizada para planear una trayectoria de espacio articular 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 aseguran que la pinza se acerque a la copa en línea recta y que la pinza permanezca a una distancia segura de la mesa, sin necesidad de que las poses de la pinza se determinen de antemano.

Configurar el modelo de robot

Este ejemplo utiliza un modelo del KUKA LBR iiwa, un manipulador de robot 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).importrobotrobotics.RigidBodyTree

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

Defina las cotas de la Copa.

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

Añadir un cuerpo fijo al modelo de robot que representa el centro de la Copa.

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

Defina 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 de inicio

  • No hay cambios bruscos en la configuración del robot

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

  • La pinza debe alinearse con la Copa a medida que se aproxima

  • Terminar con la pinza 5 cm del centro de la Copa

Este ejemplo utiliza objetos de restricción para generar configuraciones de robot que satisfacen estos criterios. La trayectoria generada consta de cinco waypoints de configuración. El primer waypoint, se establece como la configuración de inicio.q0 Asigne previamente el resto de las configuraciones en el uso.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 objetivo de posición: especifica la posición de la copa en relación con la pinza.

  • Una restricción de puntería: alinea la pinza con el eje de la Copa

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

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

gik = robotics.GeneralizedInverseKinematics('RigidBodyTree', lbr, ...     'ConstraintInputs', {'cartesian','position','aiming','orientation','joint'})
gik =    robotics.GeneralizedInverseKinematics with properties:        NumConstraints: 5     ConstraintInputs: {1x5 cell}        RigidBodyTree: [1x1 robotics.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 el agarre sea de 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 = robotics.CartesianBounds(gripper); heightAboveTable.Bounds = [-inf, inf; ...                            -inf, inf; ...                            0.05, inf]
heightAboveTable =    CartesianBounds with properties:          EndEffector: 'iiwa_link_ee_kuka'       ReferenceBody: ''     TargetTransform: [4x4 double]              Bounds: [3x2 double]             Weights: [1 1 1]  

Crear una restricción en la posición de la Copa con respecto a la pinza, con una tolerancia de 5 mm.

distanceFromCup = robotics.PositionTarget('cupFrame'); distanceFromCup.ReferenceBody = gripper; distanceFromCup.PositionTolerance = 0.005
distanceFromCup =    PositionTarget with properties:            EndEffector: 'cupFrame'         ReferenceBody: 'iiwa_link_ee_kuka'        TargetPosition: [0 0 0]     PositionTolerance: 0.0050               Weights: 1  

Cree una restricción de puntería que requiera que el eje z del fotograma sea aproximadamente vertical, colocando el objetivo muy por encima del robot.iiwa_link_ee El marco está orientado de tal forma que esta restricción alinea la pinza con el eje de la Copa.iiwa_link_ee

alignWithCup = robotics.AimingConstraint('iiwa_link_ee'); alignWithCup.TargetPoint = [0, 0, 100]
alignWithCup =    AimingConstraint 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 las uniones.Bounds

limitJointChange = robotics.JointPositionBounds(lbr)
limitJointChange =    JointPositionBounds 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 la orientación del agarre para que 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 a la Copa.

fixOrientation = robotics.OrientationTarget(gripper); fixOrientation.OrientationTolerance = deg2rad(1)
fixOrientation =    OrientationTarget with properties:               EndEffector: 'iiwa_link_ee_kuka'            ReferenceBody: ''        TargetOrientation: [1 0 0 0]     OrientationTolerance: 0.0175                  Weights: 1  

Encuentra una configuración que apunte a la Copa

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

intermediateDistance = 0.3;

Los objetos de restricción tienen una propiedad que determina cómo el solucionador trata las restricciones conflictivas.Weights Al establecer los pesos de una restricción en cero, se 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;

Fije la posición objetivo para la copa en el bastidor del agarre. La Copa debe estar sobre el eje z de la pinza a la distancia especificada.

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

Resuelva la configuración del robot que satisfaga 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);

Encuentre configuraciones que muevan la pinza a la Copa a lo largo de una línea recta

Vuelva a activar las restricciones de posición de Unión y de orientación.

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

Deshabilite la restricción alinear con copa, 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,:) Obtener la transformación de la pinza a la base del modelo de robot. Convierta la transformación homogénea en un cuaternión.

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

Define la distancia entre la taza y la pinza para cada waypoint

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

Defina el cambio máximo permitido en las posiciones conjuntas 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

Visualice la trayectoria generada

Interpolar entre los waypoints para generar una trayectoria suave. Se utiliza para evitar los brotes, que podrían violar los límites de la articulación del robot.pchip

framerate = 15; r = robotics.Rate(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 agarre 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 Copa

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

Anima el manipulador y traza la posición de agarre.

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