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.

Compruebe si hay colisiones ambientales con manipuladores

Genere una trayectoria libre de colisiones en un espacio de trabajo restringido.

Definir un entorno

Puede crear un entorno sencillo mediante primitivas de colisión. Por ejemplo, supongamos que el robot está en un espacio de trabajo donde el objetivo es mover objetos de una mesa a otra evitando una luminaria circular. Estos objetos se pueden modelar como dos cuadros y una esfera. Se pueden crear entornos más complejos utilizando objetos.collisionMesh

% Create two platforms platform1 = collisionBox(0.5,0.5,0.25); platform1.Pose = trvec2tform([-0.5 0.4 0.2]);  platform2 = collisionBox(0.5,0.5,0.25); platform2.Pose = trvec2tform([0.5 0.2 0.2]);  % Add a light fixture, modeled as a sphere lightFixture = collisionSphere(0.1); lightFixture.Pose = trvec2tform([.2 0 1]);  % Store in a cell array for collision-checking worldCollisionArray = {platform1 platform2 lightFixture};

Visualice el entorno mediante una función auxiliar que recorre en iteración la matriz de colisiones.

exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);

Añadir un robot manipulador

Añadir un manipulador Kinova al medio ambiente en el origen. Cargue el modelo de robot proporcionado. Visualiza los obstáculos y muestra el robot en la misma figura.

robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]); ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray); show(robot,homeConfiguration(robot),"Parent",ax);

Modele el manipulador como una matriz de objetos de colisión

Cree una matriz de objetos de colisión a partir del objeto.rigidBodyTree Este enfoque utiliza una aplicación auxiliar de ejemplo, , que extrae las mallas del primer objeto visual de cada objeto.exampleHelperManipCollisionsFromVisualsrigidBody Para obtener una visión general de otros enfoques, consulte .Crear objetos de colisión para la comprobación de colisión del manipulador

% Generate an array of collision objects from the visuals of the associated tree collisionArray = exampleHelperManipCollisionsFromVisuals(robot);

Generar una trayectoria y comprobar si hay colisiones

Defina una pose inicial y final como posición y orientación. Se utiliza para resolver las posiciones de unión en función de las posturas deseadas.inverseKinematics Inspeccione manualmente para verificar que las configuraciones son válidas.

startPose = trvec2tform([-0.5,0.5,0.4])*axang2tform([1 0 0 pi]); endPose = trvec2tform([0.5,0.2,0.4])*axang2tform([1 0 0 pi]);  % Use a fixed random seed to ensure repeatable results rng(0); ik = inverseKinematics("RigidBodyTree",robot); weights = ones(1,6); startConfig = ik("EndEffector_Link",startPose,weights,robot.homeConfiguration); endConfig = ik("EndEffector_Link",endPose,weights,robot.homeConfiguration);  % Show initial and final positions show(robot,startConfig); show(robot,endConfig);

Utilice un perfil de velocidad trapezoidal para generar una trayectoria suave desde la posición inicial hasta la posición inicial y, a continuación, hasta la posición final. Utilice la comprobación de colisiones para ver si esto da lugar a colisiones.

q = trapveltraj([homeConfiguration(robot),startConfig,endConfig],200,"EndTime",2);  %Initialize outputs isCollision = false(length(q),1); % Check whether each pose is in collision selfCollisionPairIdx = cell(length(q),1); % Provide the bodies that are in collision worldCollisionPairIdx = cell(length(q),1); % Provide the bodies that are in collision  for i = 1:length(q)     [isCollision(i),selfCollisionPairIdx{i},worldCollisionPairIdx{i}] = exampleHelperManipCheckCollisions(robot,collisionArray,worldCollisionArray,q(:,i),false); end isTrajectoryInCollision = any(isCollision)
isTrajectoryInCollision = logical
   1

Inspeccionar los casos problemáticos

Al inspeccionar las colisiones, se producen 2 colisiones. Visualice estas configuraciones para investigar más a fondo.

problemIdx1 = find(isCollision,1); problemIdx2 = find(isCollision,1,"last");  % Identify the problem rigid bodies problemBodies1 = [selfCollisionPairIdx{problemIdx1} worldCollisionPairIdx{problemIdx1}*[1 0]']; problemBodies2 = [selfCollisionPairIdx{problemIdx2} worldCollisionPairIdx{problemIdx2}*[1 0]'];  % Visualize the environment ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);  % Add the robots & highlight the problem bodies show(robot,q(:,problemIdx1),"Parent",ax,"PreservePlot",false); exampleHelperHighlightCollisionBodies(robot,problemBodies1,ax); show(robot,q(:,problemIdx2),"Parent"',ax); exampleHelperHighlightCollisionBodies(robot,problemBodies2,ax);

Generar una trayectoria libre de colisiones usando waypoints intermedios

Para evitar estas colisiones, agregue puntos intermedios intermedios para asegurarse de que el robot navega alrededor del obstáculo.

intermediatePose1 = trvec2tform([-.3 -.2 .6])*axang2tform([0 1 0 -pi/4]); % Out and around the sphere intermediatePose2 = trvec2tform([0.2,0.2,0.6])*axang2tform([1 0 0 pi]); % Come in from above  intermediateConfig1 = ik("EndEffector_Link",intermediatePose1,weights,q(:,problemIdx1)); intermediateConfig2 = ik("EndEffector_Link",intermediatePose2,weights,q(:,problemIdx2));  % Show the new intermediate poses ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray); show(robot,intermediateConfig1,"Parent",ax,"PreservePlot",false); show(robot,intermediateConfig2,"Parent",ax);

Genera una nueva trayectoria.

[q,qd,qdd,t] = trapveltraj([homeConfiguration(robot),intermediateConfig1,startConfig,intermediateConfig2,endConfig],200,"EndTime",2);

Verifique que esté libre de colisiones.

%Initialize outputs isCollision = false(length(q),1); % Check whether each pose is in collision collisionPairIdx = cell(length(q),1); % Provide the bodies that are in collision for i = 1:length(q)     [isCollision(i),collisionPairIdx{i}] = exampleHelperManipCheckCollisions(robot,collisionArray,worldCollisionArray,q(:,i),false); end isTrajectoryInCollision = any(isCollision)
isTrajectoryInCollision = logical
   0

Visualizar la trayectoria generada

Animar el resultado.

% Plot the environment ax2 = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);  % Visualize the robot in its home configuration show(robot,startConfig,"Parent",ax2);  % Update the axis size axis equal  % Loop through the other positions for i = 1:length(q)     show(robot,q(:,i),"Parent",ax2,"PreservePlot",false);          % Update the figure         drawnow end

Trazar las posiciones de las articulaciones a lo largo del tiempo.

figure plot(t,q) xlabel("Time") ylabel("Joint Position")