Planificar una trayectoria de alcance con múltiples restricciones cinemáticas
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));
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
Si desea guardar las configuraciones generadas en un archivo MAT para su uso posterior, ejecute lo siguiente:
>> save('lbr_trajectory.mat', 'tWaypoints', 'qWaypoints');