Учитывая набор желаемой объединенной настройки waypoints и управляемого крутящим моментом манипулятора, этот пример показывает, как реализовать контроллер вычисленного крутящего момента, использующий функцию inverseDynamics. Контроллер позволяет роботу следовать за данной настройкой waypoints вдоль сглаженной траектории.
Породите робота LBR в Средстве моделирования Gazebo. Выполните шаги в Запуске с Gazebo и Моделируемым TurtleBot, чтобы запустить Gazebo LBR Simulator с рабочего стола виртуальной машины Ubuntu®.
Gazebo LBR Simulator поднимает базового Робота Легкого веса KUKA (LBR) манипулятор без положения по умолчанию, скорости или контроллеров безопасности. Единственный способ переместить робота через объединенные крутящие моменты. Если симуляция начинает запускаться, рука LBR упадет на землю ни из-за какого объединенного входа крутящего момента.
В вашем экземпляре 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 = 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.matcdt является запланированным управлением 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 обеспечивает два сервиса 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');
Используйте обеспеченный 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.
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