Поднимите LBR симуляция Gazebo

Породите робота LBR в Средстве моделирования Gazebo. Выполните шаги в Начало работы с Gazebo и Симулированным TurtleBot (ROS Toolbox), чтобы запустить Gazebo LBR Simulator с рабочего стола виртуальной машины Ubuntu®.

Gazebo LBR Simulator поднимает базового Робота Легкого веса KUKA (LBR) манипулятор без положения по умолчанию, скорости или контроллеров безопасности. Единственный способ переместить робота через объединенные крутящие моменты. Если симуляция начинает запускаться, рука LBR упадет на землю ни из-за какого объединенного входа крутящего момента.

Соединитесь с сетью ROS от MATLAB®

В вашем экземпляре MATLAB на хосте - компьютере запустите следующие команды, чтобы инициализировать глобальный узел ROS в MATLAB и соединиться с ведущим устройством ROS в виртуальной машине (куда Gazebo запускается) через его IP-адрес. Замените ipaddress с IP-адресом вашей виртуальной машины.

ipaddress = '192.168.233.133';
rosinit(ipaddress,11311);

Initializing global node /matlab_global_node_56016 with NodeURI http://192.168.233.1:55980/

Создайте LBR rigidBodyTree Объект от URDF

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

Установите силу тяжести совпадать с этим в Gazebo.

lbr.Gravity = [0 0 -9.80];

Покажите домашнюю настройку в фигуре MATLAB.

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

Предварительно вычислите траекторию крутящего момента соединения для желаемого движения

Загрузите объединенную настройку waypoints. Это дает ключевые кадры для желаемого движения робота.

load lbr_waypoints.mat

cdt запланированное управление stepsize. Мы используем его, чтобы заполнить набор моментов времени, где траектория должна быть оценена и сохранить его в векторном tt.

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

Сгенерируйте желаемую траекторию движения для каждого соединения. exampleHelperJointTrajectoryGeneration генерирует объединенные траектории с данного времени и объединенную настройку waypoints.

Траектории сгенерированы с помощью pchip так, чтобы интерполированное объединенное положение не нарушало объединенные пределы, пока waypoints не делают.

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

Предварительно вычислите крутящие моменты feedforward, которые идеально поняли бы желаемое движение (принимающий воздействия или любой вид ошибок) использование inverseDynamics. Следующий for цикл занимает время, чтобы запуститься. Чтобы ускориться, рассмотрите используемый сгенерированный код для inverseDynamics. Смотрите последний раздел для получения дополнительной информации о том, как сделать это.

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

Установите канал связи с Gazebo через индивидуально настраиваемые темы

Gazebo предоставляет две услуги ROS /gazebo/get_joint_properties и /gazebo/apply_joint_effort это может использоваться, чтобы получить объединенное состояние и установить объединенные крутящие моменты. Однако сервисы также не спешат закрывать цикл управления крутящего момента. Поэтому индивидуально настраиваемый плагин Gazebo используется так, чтобы объединенное состояние/крутящие моменты в Gazebo могло читаться/писаться на намного более быстром уровне через плоскость темы ROS (издатель и подписчик). Индивидуально настраиваемый плагин Gazebo уже поднят вместе с Gazebo LBR Simulator.

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

Сбросьте LBR, чтобы разместить настройку в Gazebo

Используйте обеспеченный Gazebo сервис сбросить робота к его домашней настройке. Для получения дополнительной информации о том, как работать с сервисом ROS в MATLAB, смотрите Call and Provide ROS Services (ROS Toolbox).

mdlConfigClient = rossvcclient('gazebo/set_model_configuration');

Сочините необходимое сервисное сообщение. Это включает объединенные имена и соответствующие объединенные положения, чтобы отправить к Gazebo. Вызовите сервис с помощью этого сообщения.

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

Вычисленное управление крутящим моментом

Задайте усиления PD.

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

once = 1;

Подготовьтесь к регистрации данных.

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

Вычисленное управление крутящим моментом реализовано в for цикл ниже. Как только MATLAB получает новое объединенное состояние от Gazebo, он ищет в предсгенерированном tauFeedForward и находит крутящий момент feedforward, соответствующий метке времени. Это также вычисляет крутящий момент PD, чтобы компенсировать ошибки в объединенном положении и скоростях [1].

С настройками по умолчанию в Gazebo, /iiwa_matlab_plugin/iiwa_matlab_joint_state тема обновляется на уровне приблизительно 1 кГц (Gazebo sim время) с типичными 0,6 оперативными факторами. И цикл управления крутящего момента ниже может обычно запускаться на уровне приблизительно 200 Гц (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

С объединенными отправленными крутящими моментами робот LBR должен следовать за траекторией. Это изображение показывает снимки состояния робота, наложенного в траектории.

Смотрите результаты

Постройте и смотрите фактические объединенные крутящие моменты и положения по сравнению с требуемыми значениями. Обратите внимание на то, что с крутящим моментом feedforward, крутящие моменты PD должны колебаться вокруг нуля.

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

Завершите работу сети ROS.

Отключитесь от робота и завершите работу сети ROS.

rosshutdown

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

Генерация кода для обратной динамики

Чтобы ускорить вычисление крутящего момента в цикле, сгенерируйте код для inverseDynamics функция.

Создайте функцию под названием invDyn. Отметьте exampleHelperMwIiwa14 codegen-совместимая функция, которая воссоздает то же самое rigidBodyTree возразите как возвращенный importrobot('iiwa14.urdf').

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

Затем используйте следующий codegen команда

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

Наконец, со сгенерированным invDyn_mex файл, можно заменить inverseDynamics вызовите в for цикл

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

с

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

Смотрите также

Ссылки

[1] Б. Сисилано, Л. Скиэвикко, Л. Виллэни, Г. Орайоло, "робототехника: моделирование, планируя и управление", Спрингер, 2009