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 y ejecutar trayectorias de espacio de tareas y articulaciones utilizando el manipulador KINOVA Gen3

En este ejemplo se muestra cómo generar y simular trayectorias de unión interpoladas para pasar de una postura inicial a una postura de efecto final deseada. El tiempo de las trayectorias se basa en una velocidad aproximada deseada de la herramienta de extremo del brazo (EOAT).

Cargue el modelo de robot de árbol de cuerpo rígido (RBT) KINOVA Gen3.

robot = loadrobot('kinovaGen3','DataFormat','row','Gravity',[0 0 -9.81]);

Establezca la configuración actual de la junta del robot.

currentRobotJConfig = homeConfiguration(robot);

Obtenga el número de articulaciones y el marco RBT del efecto final.

numJoints = numel(currentRobotJConfig); endEffector = "EndEffector_Link";

Especifique el paso de tiempo de trayectoria y la velocidad de herramienta deseada aproximada.

timeStep = 0.1; % seconds toolSpeed = 0.1; % m/s

Establezca la pose inicial y final del efector final.

jointInit = currentRobotJConfig; taskInit = getTransform(robot,jointInit,endEffector);  taskFinal = trvec2tform([0.4,0,0.6])*axang2tform([0 1 0 pi]);

Generar trayectoria de espacio de tareas

Calcular waypoints de trayectoria de espacio de tareas a través de la interpolación.

En primer lugar, la distancia de viaje de la herramienta de cálculo.

distance = norm(tform2trvec(taskInit)-tform2trvec(taskFinal));

A continuación, defina los tiempos de trayectoria en función de la distancia de viaje y la velocidad deseada de la herramienta.

initTime = 0; finalTime = (distance/toolSpeed) - initTime; trajTimes = initTime:timeStep:finalTime; timeInterval = [trajTimes(1); trajTimes(end)];

Interpolar entre puntos intermedios de espacio de tareas intermedios y para calcularlos intermedios.taskInittaskFinal

[taskWaypoints,taskVelocities] = transformtraj(taskInit,taskFinal,timeInterval,trajTimes); 

Controlar el movimiento tarea-espacio

Cree un modelo de movimiento de espacio de unión para el control de PD en las uniones. El objeto modela el movimiento de un modelo de árbol de sólido rígido bajo el control proporcional-derivado del espacio de tareas.taskSpaceMotionModel

tsMotionModel = taskSpaceMotionModel('RigidBodyTree',robot,'EndEffectorName','EndEffector_Link');

Establezca las ganancias proporcionales y derivadas en la orientación en cero, de modo que el comportamiento controlado siga las posiciones de referencia:

tsMotionModel.Kp(1:3,1:3) = 0; tsMotionModel.Kd(1:3,1:3) = 0;

Definir los estados iniciales (posiciones y velocidades conjuntas).

q0 = currentRobotJConfig;  qd0 = zeros(size(q0));

Se utiliza para simular el movimiento del robot.ode15s Puesto que el estado de referencia cambia en cada instante, se requiere una función de envoltorio para actualizar la entrada de trayectoria interpolada a la derivada de estado en cada instante. Por lo tanto, una función auxiliar de ejemplo se pasa como el identificador de función al solucionador de ODE. Los estados del manipulador resultantes se emiten en .stateTask

[tTask,stateTask] = ode15s(@(t,state) exampleHelperTimeBasedTaskInputs(tsMotionModel,timeInterval,taskInit,taskFinal,t,state),timeInterval,[q0; qd0]);

Generar trayectoria de espacio conjunto

Cree un objeto cinemático inverso para el robot.

ik = inverseKinematics('RigidBodyTree',robot); ik.SolverParameters.AllowRandomRestart = false; weights = [1 1 1 1 1 1];

Calcule las configuraciones de juntas iniciales y deseadas utilizando cinemática inversa.

initialGuess = wrapToPi(jointInit); jointFinal = ik(endEffector,taskFinal,weights,initialGuess); jointFinal = wrapToPi(jointFinal);

Interpolar entre ellos utilizando una función polinómica cúbica para generar una matriz de configuraciones de juntas de espaciado uniforme. Utilice una b-spline para generar una trayectoria suave.

ctrlpoints = [jointInit',jointFinal']; jointConfigArray = cubicpolytraj(ctrlpoints,timeInterval,trajTimes); jointWaypoints = bsplinepolytraj(jointConfigArray,timeInterval,1);

Controlar la trayectoria del espacio conjunto

Cree un modelo de movimiento de espacio de unión para el control de PD en las uniones. El objeto modela el movimiento de un modelo de árbol de sólido rígido y utiliza un control proporcional de derivados en las posiciones de unión especificadas.jointSpaceMotionModel

jsMotionModel = jointSpaceMotionModel('RigidBodyTree',robot,'MotionType','PDControl');

Establecer estados iniciales (posiciones conjuntas y velocidades).

q0 = currentRobotJConfig;  qd0 = zeros(size(q0));

Se utiliza para simular el movimiento del robot.ode15s Una vez más, se utiliza una función auxiliar de ejemplo como entrada de identificador de función para el solucionador de ODE con el fin de actualizar las entradas de referencia en cada instante en el tiempo. Los estados de espacio de unión se generan en .stateJoint

[tJoint,stateJoint] = ode15s(@(t,state) exampleHelperTimeBasedJointInputs(jsMotionModel,timeInterval,jointConfigArray,t,state),timeInterval,[q0; qd0]);

Visualizar y comparar trayectorias de robots

Mostrar la configuración inicial del robot.

show(robot,currentRobotJConfig,'PreservePlot',false,'Frames','off'); hold on axis([-1 1 -1 1 -0.1 1.5]);

Visualice la trayectoria del espacio de tareas. Repasar en iteración los estados e interpolar en función de la hora actual.stateTask

for i=1:length(trajTimes)     % Current time      tNow= trajTimes(i);     % Interpolate simulated joint positions to get configuration at current time     configNow = interp1(tTask,stateTask(:,1:numJoints),tNow);     poseNow = getTransform(robot,configNow,endEffector);     show(robot,configNow,'PreservePlot',false,'Frames','off');     plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'b.','MarkerSize',20)     drawnow; end

Visualice la trayectoria del espacio conjunto. Repasar en iteración los estados e interpolar en función de la hora actual.jointTask

% Return to initial configuration show(robot,currentRobotJConfig,'PreservePlot',false,'Frames','off');  for i=1:length(trajTimes)     % Current time      tNow= trajTimes(i);     % Interpolate simulated joint positions to get configuration at current time     configNow = interp1(tJoint,stateJoint(:,1:numJoints),tNow);     poseNow = getTransform(robot,configNow,endEffector);     show(robot,configNow,'PreservePlot',false,'Frames','off');     plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'r.','MarkerSize',20)     drawnow; end