Trazado de rutas en 2D con cinemática inversa

Introducción

Este ejemplo muestra cómo calcular la cinemática inversa de un manipulador 2D sencillo utilizando la clase inverseKinematics. El robot manipulador es un manipulador plano simple con dos grados de libertad y articulaciones giratorias, que se crea ensamblando cuerpos rígidos en un objeto rigidBodyTree. Se crea una trayectoria circular en un plano en 2D y se introduce en forma de puntos en el solver de cinemática inversa. El solver calcula las posiciones de articulación necesarias para obtener esta trayectoria. Finalmente, el robot se anima para mostrar las configuraciones que permiten obtener la trayectoria circular.

Construir el robot

Cree un objeto rigidBodyTree y cuerpos rígidos con sus articulaciones asociadas. Especifique las propiedades geométricas de cada cuerpo rígido y añádalas al robot.

Comience con un modelo de árbol de cuerpo rígido en blanco.

robot = rigidBodyTree('DataFormat','column','MaxNumBodies',3);

Especifique las longitudes del brazo robótico.

L1 = 0.3;
L2 = 0.3;

Añada un cuerpo 'link1' con una articulación 'joint1'.

body = rigidBody('link1');
joint = rigidBodyJoint('joint1', 'revolute');
setFixedTransform(joint,trvec2tform([0 0 0]));
joint.JointAxis = [0 0 1];
body.Joint = joint;
addBody(robot, body, 'base');

Añada un cuerpo 'link2' con una articulación 'joint2'.

body = rigidBody('link2');
joint = rigidBodyJoint('joint2','revolute');
setFixedTransform(joint, trvec2tform([L1,0,0]));
joint.JointAxis = [0 0 1];
body.Joint = joint;
addBody(robot, body, 'link1');

Añada un efector final 'tool' con una articulación fija 'fix1'.

body = rigidBody('tool');
joint = rigidBodyJoint('fix1','fixed');
setFixedTransform(joint, trvec2tform([L2, 0, 0]));
body.Joint = joint;
addBody(robot, body, 'link2');

Muestre los detalles del robot para validar las propiedades de entrada. El robot debe tener dos articulaciones no fijas en las que los cuerpos de enlace se conectan a sus cuerpos principales mediante una articulación giratoria y el efector final se conecta a su cuerpo principal mediante una articulación fija.

showdetails(robot)

--------------------
Robot: (3 bodies)

 Idx    Body Name    Joint Name    Joint Type    Parent Name(Idx)   Children Name(s)
 ---    ---------    ----------    ----------    ----------------   ----------------
   1        link1        joint1      revolute             base(0)   link2(2)  
   2        link2        joint2      revolute            link1(1)   tool(3)  
   3         tool          fix1         fixed            link2(2)   
--------------------

Definir la trayectoria

Defina el círculo que se trazará durante 10 segundos. Este círculo está en el plano xy y tiene un radio de 0,15.

t = (0:0.2:10)'; % Time
count = length(t);
center = [0.3 0.1 0];
radius = 0.15;
theta = t*(2*pi/t(end));
points = center + radius*[cos(theta) sin(theta) zeros(size(theta))];

Soluciones de cinemática inversa

Utilice un objeto inverseKinematics para hallar una solución de configuración robótica que permita lograr las posiciones del efector final determinadas a lo largo de la trayectoria.

Predefina soluciones de configuración como matrices de qs.

q0 = homeConfiguration(robot);
ndof = length(q0);
qs = zeros(count, ndof);

Cree el solver de cinemática inversa. Dado que los puntos cartesianos xy son los únicos factores importantes de la pose del efector final en este flujo de trabajo, especifique una ponderación distinta de cero para los elementos cuarto y quinto del vector weight. El resto de los elementos se establecen en cero.

ik = inverseKinematics('RigidBodyTree', robot);
weights = [0, 0, 0, 1, 1, 0];
endEffector = 'tool';

Recorra la trayectoria de puntos para trazar el círculo. Llame al objeto ik en cada punto para generar la configuración de articulación que permite alcanzar la posición del efector final. Guarde las configuraciones para usarlas más adelante.

qInitial = q0; % Use home configuration as the initial guess
for i = 1:count
    % Solve for the configuration satisfying the desired end effector
    % position
    point = points(i,:);
    qSol = ik(endEffector,trvec2tform(point),weights,qInitial);
    % Store the configuration
    qs(i,:) = qSol;
    % Start from prior solution
    qInitial = qSol;
end

Animar la solución

Represente el robot en cada fotograma de la solución utilizando esa configuración de robot específica. Represente también la trayectoria deseada.

Muestre el robot en la primera configuración de la trayectoria. Ajuste la gráfica para mostrar el plano en 2D en el que se dibuja el círculo. Represente la trayectoria deseada.

figure
show(robot,qs(1,:)');
view(2)
ax = gca;
ax.Projection = 'orthographic';
hold on
plot(points(:,1),points(:,2),'k')
axis([-0.1 0.7 -0.3 0.5])

Configure un objeto rateControl para mostrar la trayectoria del robot a una velocidad fija de 15 fotogramas por segundo. Muestre el robot en todas las configuraciones desde el solver de cinemática inversa. Observe mientras el brazo describe la trayectoria circular mostrada.

framesPerSecond = 15;
r = rateControl(framesPerSecond);
for i = 1:count
    show(robot,qs(i,:)','PreservePlot',false);
    drawnow
    waitfor(r);
end

Consulte también

rigidBodyTree | rigidBody | rigidBodyJoint | inverseKinematics