Документация

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

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

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

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

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

ipaddress = '192.168.203.129';
rosinit(ipaddress);

Initializing global node /matlab_global_node_46761 with NodeURI http://192.168.203.1:49420/

Создайте объект 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);

Предварительно вычислите прямые каналом крутящие моменты, которые идеально поняли бы желаемое движение (принимающий воздействия или любой вид ошибок) использующий 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.

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 и находит прямой каналом крутящий момент, соответствующий метке времени. Это также вычисляет крутящий момент 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 должен следовать за траекторией. Это изображение показывает снимки состояния робота, наложенного в траектории.

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

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

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

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

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

rosshutdown

Shutting down global node /matlab_global_node_46761 with NodeURI http://192.168.203.1:49420/

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

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

Создайте функцию под названием invDyn. Обратите внимание, что exampleHelperMwIiwa14 является codegen-совместимой функцией, которая воссоздает тот же объект robotics.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