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.

Controle el movimiento del manipulador LBR a través de comandos de par conjunto

Dado un conjunto de waypoints de configuración de juntas deseados y un manipulador controlado por par, este ejemplo muestra cómo implementar el controlador de par calculado utilizando la función.inverseDynamics El controlador permite al robot seguir los puntos de ruta de configuración dados a lo largo de una trayectoria suave.

Traiga la simulación LBR Gazebo

Engendra un robot LBR en Gazebo Simulator. Siga los pasos para iniciar el desde el Ubuntu® escritorio de la máquina virtual.Get Started with Gazebo and a Simulated TurtleBot (ROS Toolbox)Gazebo LBR Simulator

trae un manipulador de robot ligero KUKA (LBR) sin controladores de posición, velocidad o seguridad predeterminados.Gazebo LBR Simulator La única manera de mover el robot es a través de pares de articulaciones. Una vez que la simulación comienza a funcionar, el brazo LBR caerá sobre el suelo debido a que no hay entrada de par de unión.

Conéctese a ros Network desde MATLAB®

En la instancia de MATLAB en el equipo host, ejecute los siguientes comandos para inicializar el nodo global ROS en MATLAB y conectarse al maestro ROS en la máquina virtual (donde se ejecuta Gazebo) a través de su dirección IP. Reemplace por la dirección IP de la máquina virtual.ipaddress

ipaddress = '192.168.233.133'; rosinit(ipaddress,11311);
Initializing global node /matlab_global_node_56016 with NodeURI http://192.168.233.1:55980/ 

Cree un objeto LBR de URDFrigidBodyTree

lbr = importrobot('iiwa14.urdf'); lbr.DataFormat = 'row';

Establezca la gravedad para que sea la misma que la de Gazebo.

lbr.Gravity = [0 0 -9.80];

Mostrar la configuración del hogar en una figura de MATLAB.

show(lbr); view([150 12]); axis([-0.6 0.6 -0.6 0.6 0 1.35]); camva(9); daspect([1 1 1]);

Trayectoria de par de junta pre-computación para movimiento deseado

Cargue los waypoints de configuración de la junta. Esto proporciona los fotogramas clave para el movimiento deseado del robot.

load lbr_waypoints.mat

es el tamaño escalonado de control planificado.cdt Lo usamos para rellenar un conjunto de puntos de tiempo donde la trayectoria necesita ser evaluada y almacenarla en vector.tt

cdt = 0.001;  tt = 0:cdt:5;

Genere la trayectoria de movimiento deseada para cada articulación. genera trayectorias conjuntas a partir de puntos intermedios de configuración de tiempo y juntas dados.exampleHelperJointTrajectoryGeneration

Las trayectorias se generan utilizando para que la posición de unión interpolada no viole los límites de unión siempre y cuando los waypoints no lo hagan.pchip

[qDesired, qdotDesired, qddotDesired, tt] = exampleHelperJointTrajectoryGeneration(tWaypoints, qWaypoints, tt);

Pre-calcular los pares de avance de avance de avance de avance que idealmente realizarían el movimiento deseado (suponiendo que no haya perturbaciones o cualquier tipo de errores) utilizando .inverseDynamics El siguiente bucle tarda algún tiempo en ejecutarse.for Para acelerar, considere el código generado utilizado para .inverseDynamics Consulte la última sección para obtener más información sobre cómo hacerlo.

n = size(qDesired,1); tauFeedForward = zeros(n,7); for i = 1:n     tauFeedForward(i,:) = inverseDynamics(lbr, qDesired(i,:), qdotDesired(i,:), qddotDesired(i,:)); end

Establecer canal de comunicación con Gazebo a través de temas personalizados

Gazebo proporciona dos servicios ROS y que se pueden utilizar para obtener el estado de la junta y establecer pares de par de unión./gazebo/get_joint_properties/gazebo/apply_joint_effort Sin embargo, los servicios son demasiado lentos para cerrar el bucle de control de par. Por lo tanto, se utiliza un plug-in Gazebo personalizado para que el estado/torques de la junta en Gazebo se pueda leer/escribir a un ritmo mucho más rápido a través de los temas ROS simples (editor y suscriptor). El plugin Gazebo personalizado ya se ha creado junto con .Gazebo LBR Simulator

[jointTauPub, jtMsg] = rospublisher('/iiwa_matlab_plugin/iiwa_matlab_joint_effort'); jointStateSub = rossubscriber('/iiwa_matlab_plugin/iiwa_matlab_joint_state');

Restablecer LBR a la configuración del hogar en Gazebo

Utilice el servicio proporcionado por Gazebo para restablecer el robot a su configuración doméstica. Para obtener más información sobre cómo trabajar con el servicio ROS en MATLAB, consulte .Call and Provide ROS Services (ROS Toolbox)

mdlConfigClient = rossvcclient('gazebo/set_model_configuration');

Componga el mensaje de servicio necesario. Incluye los nombres conjuntos y las posiciones conjuntas correspondientes para enviar a Gazebo. Llame al servicio mediante este mensaje.

msg = rosmessage(mdlConfigClient); msg.ModelName = 'mw_iiwa'; msg.UrdfParamName = 'robot_description'; msg.JointNames = {'mw_iiwa_joint_1', 'mw_iiwa_joint_2', 'mw_iiwa_joint_3',...                   'mw_iiwa_joint_4', 'mw_iiwa_joint_5', 'mw_iiwa_joint_6', 'mw_iiwa_joint_7'}; msg.JointPositions = homeConfiguration(lbr);  call(mdlConfigClient, msg)
ans =    ROS SetModelConfigurationResponse message with properties:        MessageType: 'gazebo_msgs/SetModelConfigurationResponse'           Success: 1     StatusMessage: 'SetModelConfiguration: success'    Use showdetails to show the contents of the message  

Control de par computado

Especifique las ganancias de PD.

weights = [0.3,0.8,0.6, 0.6,0.3,0.2,0.1]; Kp = 100*weights; Kd = 2* weights;  once = 1;

Prepárese para el registro de datos.

feedForwardTorque = zeros(n, 7); pdTorque = zeros(n, 7); timePoints = zeros(n,1); Q = zeros(n,7); QDesired = zeros(n,7);

El control de par calculado se implementa en el bucle siguiente.for Tan pronto como MATLAB recibe un nuevo estado de unión de Gazebo, mira hacia arriba en el pre-generado y encuentra el par de avance de avance de avance de avance correspondiente a la marca de tiempo.tauFeedForward También calcula un par PD para compensar los errores en la posición de la articulación y las velocidades [1].

Con la configuración predeterminada en Gazebo, el tema se actualiza en alrededor de 1 kHz (Gazebo sim time) con un factor de tiempo real típico de 0.6./iiwa_matlab_plugin/iiwa_matlab_joint_state Y el bucle de control de par a continuación normalmente puede funcionar a alrededor de 200 Hz (tiempo de gazebo sim).

for i = 1:n     % Get joint state from Gazebo.     jsMsg = receive(jointStateSub);     data = jsMsg.Data;          % Parse the received message.      % The Data in jsMsg is a 1-by-15 vector.     % 1:7  - joint positions     % 8:14 - joint velocities     % 15   - time (Gazebo sim time) when the joint state is updated     q = double(data(1:7))';     qdot = double(data(8:14))';     t = double(data(15));          % Set the start time.     if once         tStart = t;         once = 0;     end          % Find the corresponding index h in tauFeedForward vector for joint      % state time stamp t.     h = ceil((t - tStart + 1e-8)/cdt);     if h>n         break     end          % Log joint positions data.     Q(i,:) = q';     QDesired(i,:) = qDesired(h,:);          % Inquire feed-forward torque at the time when the joint state is     % updated (Gazebo sim time).     tau1 = tauFeedForward(h,:);     % Log feed-forward torque.     feedForwardTorque(i,:) = tau1;          % Compute PD compensation torque based on joint position and velocity     % errors.     tau2 = Kp.*(qDesired(h,:) - q) + Kd.*(qdotDesired(h,:) - qdot);     % Log PD torque.     pdTorque(i,:) = tau2';          % Combine the two torques.     tau = tau1 + tau2;          % Log the time.     timePoints(i) = t-tStart;          % Send torque to Gazebo.     jtMsg.Data = tau;     send(jointTauPub,jtMsg);     end

Con los pares de par de unión enviados, el robot LBR debe seguir la trayectoria. Esta imagen muestra instantáneas del robot superpuestas a lo largo de la trayectoria.

Inspeccionar resultados

Trazar e inspeccionar los pares y posiciones reales de la junta frente a los valores deseados. Tenga en cuenta que con el par de avance de alimentación, los pares PD deben oscilar alrededor de cero.

exampleHelperLBRPlot(i-1, timePoints, feedForwardTorque, pdTorque, Q, QDesired )

Apague la red ROS.

Desconecte del robot y apague la red ROS.

rosshutdown
Shutting down global node /matlab_global_node_56016 with NodeURI http://192.168.233.1:55980/ 

Generación de código para dinámica inversa

Para acelerar el cálculo del par en un bucle, genere código para la función.inverseDynamics

Cree una función denominada .invDyn Nota es una función compatible con codegen que vuelve a crear el mismo objeto que el devuelto por .exampleHelperMwIiwa14rigidBodyTreeimportrobot('iiwa14.urdf')

    function tau = invDyn( q, qdot, qddot )         %#codegen         persistent robot          if isempty(robot)              robot = exampleHelperMwIiwa14;          end          tau = robot.inverseDynamics(q, qdot, qddot);       end  

A continuación, utilice el siguiente comandocodegen

    codegen invDyn.m -args {zeros(1,7), zeros(1,7), zeros(1,7)} 

Finalmente, con el archivo generado, usted puede substituir la llamada en el loopinvDyn_mexinverseDynamicsfor

    tauFeedForward(i,:) = inverseDynamics(lbr, qDesired(i,:), qdotDesired(i,:), qddotDesired(i,:)); 

Con

    tauFeedForward(i,:) = invDyn_mex(qDesired(i,:), qdotDesired(i,:), qddotDesired(i,:)); 

Ver también

Referencias

[1] B. Sicilano, L. Sciavicco, L. Villani, G. Oriolo, "Robotics: Modelado, Planificación y Control", Springer, 2009