Planificar y ejecutar trayectorias de espacio cartesiano o articular utilizando el manipulador KINOVA Gen3
Este ejemplo muestra cómo generar y simular trayectorias de articulación interpoladas para pasar de una pose inicial a una pose deseada del efector final. La temporización de las trayectorias se basa en una velocidad deseada aproximada de la herramienta al final del brazo robótico (EOAT).
Cargue el modelo de robot de árbol de cuerpo rígido (RBT) de KINOVA Gen3.
robot = loadrobot('kinovaGen3','DataFormat','row','Gravity',[0 0 -9.81]);
Defina la configuración actual de la articulación del robot.
currentRobotJConfig = homeConfiguration(robot);
Obtenga el número de articulaciones y el marco de RBT del efector final.
numJoints = numel(currentRobotJConfig);
endEffector = "EndEffector_Link";
Especifique la unidad de tiempo de la trayectoria y la velocidad aproximada deseada de la herramienta.
timeStep = 0.1; % seconds toolSpeed = 0.1; % m/s
Defina 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 una trayectoria de espacio cartesiano
Calcule los waypoints de la trayectoria de espacio cartesiano mediante interpolación.
Primero, calcule la distancia de desplazamiento de la herramienta.
distance = norm(tform2trvec(taskInit)-tform2trvec(taskFinal));
Luego, defina los tiempos de trayectoria a partir de la distancia de desplazamiento y la velocidad deseada de la herramienta.
initTime = 0; finalTime = (distance/toolSpeed) - initTime; trajTimes = initTime:timeStep:finalTime; timeInterval = [trajTimes(1); trajTimes(end)];
Interpole entre taskInit
y taskFinal
para calcular los waypoints intermedios de espacio cartesiano.
[taskWaypoints,taskVelocities] = transformtraj(taskInit,taskFinal,timeInterval,trajTimes);
Controlar el movimiento en el espacio cartesiano
Cree un modelo de movimiento en el espacio cartesiano para el control PD en las articulaciones. El objeto taskSpaceMotionModel
modela el movimiento de un modelo de árbol de cuerpo rígido bajo un control proporcional derivativo del espacio cartesiano.
tsMotionModel = taskSpaceMotionModel('RigidBodyTree',robot,'EndEffectorName','EndEffector_Link');
Establezca en cero la ganancia proporcional y derivada en la orientación, de modo que el comportamiento controlado simplemente sigue las posiciones de referencia:
tsMotionModel.Kp(1:3,1:3) = 0; tsMotionModel.Kd(1:3,1:3) = 0;
Defina los estados iniciales (posiciones de las articulaciones y velocidades).
q0 = currentRobotJConfig; qd0 = zeros(size(q0));
Utilice ode15s
para simular el movimiento del robot. Para este problema, el sistema de lazo cerrado es rígido, lo que significa que existe una diferencia de escalado en algún punto del problema. Como resultado, el integrador a veces se ve obligado a realizar pasos sumamente pequeños, y un solver EDO no rígido como ode45
tardará mucho más tiempo en resolver el mismo problema. Para obtener más información, consulte Elegir un solver de ODE y Solve Stiff ODEs.
Dado que el estado de referencia cambia a cada instante, se necesita una función wrapper para actualizar la entrada de trayectoria interpolada en la derivada de estado en cada instante. Por tanto, se pasa una función helper de ejemplo como identificador de la función al solver EDO. Los estados resultantes del manipulador se muestran en stateTask
.
[tTask,stateTask] = ode15s(@(t,state) exampleHelperTimeBasedTaskInputs(tsMotionModel,timeInterval,taskInit,taskFinal,t,state),timeInterval,[q0; qd0]);
Generar una trayectoria de espacio articular
Cree un objeto de cinemática inversa para el robot.
ik = inverseKinematics('RigidBodyTree',robot);
ik.SolverParameters.AllowRandomRestart = false;
weights = [1 1 1 1 1 1];
Calcule las configuraciones de articulaciones inicial y deseada utilizando cinemática inversa. Envuelva los valores en pi para asegurarse de que la interpolación supera un dominio mínimo.
initialGuess = jointInit; jointFinal = ik(endEffector,taskFinal,weights,initialGuess);
De forma predeterminada, la solución IK respeta los límites de las articulaciones. Sin embargo, para las articulaciones continuas (articulaciones giratorias con rango infinito), los valores resultantes pueden ser innecesariamente grandes y se pueden envolver en [-pi, pi]
para asegurar que la trayectoria final cubre una distancia mínima. Dado que las articulaciones no continuas para Gen3 ya tienen límites dentro de este intervalo, basta con envolver los valores de las articulaciones en pi
. Las articulaciones continuas se asignarán al intervalo [-pi, pi]
y los otros valores se mantendrán iguales.
wrappedJointFinal = wrapToPi(jointFinal);
Interpole entre ellos utilizando una función polinómica cúbica para generar un arreglo de configuraciones de las articulaciones repartidas uniformemente. Utilice una B-spline para generar una trayectoria sin obstáculos.
ctrlpoints = [jointInit',wrappedJointFinal']; jointConfigArray = cubicpolytraj(ctrlpoints,timeInterval,trajTimes); jointWaypoints = bsplinepolytraj(jointConfigArray,timeInterval,1);
Controlar una trayectoria de espacio articular
Cree un modelo de movimiento en el espacio articular para el control PD en las articulaciones. El objeto jointSpaceMotionModel
modela el movimiento de un modelo de árbol de cuerpo rígido y utiliza el control proporcional derivativo en las posiciones especificadas de las articulaciones.
jsMotionModel = jointSpaceMotionModel('RigidBodyTree',robot,'MotionType','PDControl');
Establezca los estados iniciales (posiciones de las articulaciones y velocidades).
q0 = currentRobotJConfig; qd0 = zeros(size(q0));
Utilice ode15s
para simular el movimiento del robot. De nuevo, se usa una función helper de ejemplo como entrada de identificador de la función en el solver EDO para actualizar las entradas de referencia en cada instante dado. Los estados del espacio articular se muestran en stateJoint
.
[tJoint,stateJoint] = ode15s(@(t,state) exampleHelperTimeBasedJointInputs(jsMotionModel,timeInterval,jointConfigArray,t,state),timeInterval,[q0; qd0]);
Visualizar y comparar trayectorias de robot
Muestre 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 de espacio cartesiano. Itere a través de los estados de stateTask
e interpole basándose en el momento actual.
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'); taskSpaceMarker = plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'b.','MarkerSize',20); drawnow; end
Visualice la trayectoria de espacio articular. Itere a través de los estados de jointTask
e interpole basándose en el momento actual.
% 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'); jointSpaceMarker = plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),'r.','MarkerSize',20); drawnow; end % Add a legend and title legend([taskSpaceMarker jointSpaceMarker], {'Defined in Task-Space', 'Defined in Joint-Space'}); title('Manipulator Trajectories')