Контроллер скорости для робота Kuka

Question

Я пытаюсь довести свой конечный эффектор Куки go до желаемой цели с помощью контрольной функции Ляпунова. Я слежу за этой статьей: http://skrifo.juxi.net/papers/zhuang2019iros.pdf

Функция Ляпунова здесь , а это - ее частные производные по углам сочленения. .

Пока что контроллер, кажется, делает какие-то дурацкие вещи - функция Ляпунова не убывает на всех временных шагах [assert(V <= prev_V + 0.001) не работает!]. Функция Ляпунова кажется правильной, но я думаю, что проблема связана с вычислением частных производных. Расчет Якобиана имеет смысл в моей голове (я хочу, чтобы результат находился в конечном эффекторном кадре), но что-то явно не так:

jacobian = robot.CalcJacobianSpatialVelocity(
    context=differential_ik.GetRobotContext(), with_respect_to=JacobianWrtVariable.kQDot,
    frame_B=robot.world_frame(),                    # frame on which point Bi is fixed
    p_BP=[0.0, 0.0, 0.0],                           # position vec from frame_B's origin to points Bi
    frame_A=robot.GetFrameByName("iiwa_link_7"),    # frame that measures v_ABi
    frame_E=robot.GetFrameByName("iiwa_link_7"))    # frame that v_ABi is expressed on input 
                                                    # and frame that Jacobian J_V_ABi is expressed on output

[Я добавил GetRobotContext() в класс DifferentialIK, но мог бы использовать GetSubsystemContext()]

Я устанавливаю скорость в Drake с помощью q = q_prev + time_step * q_dot, а затем differential_ik.SetPositions() на q.

Вот самый маленький рабочий код, который я мог создать воспроизвести мою проблему (извините, она все еще большая): https://gist.github.com/maggi3wang/1cbb02a0adc2cec85915f856ab3d1702#file -small_collect_data-py-L174

Любая помощь будет принята с благодарностью!

Answer 1

Я бы сказал, что самый быстрый подход к отладке вашей программы - это проверить, правильно ли вычислено ∂V / ∂θ (что, как мне кажется, является переменной pdV в вашем коде). Вы можете вычислить этот градиент по числовой разнице, а затем сравнить числовой градиент с вашим градиентом.

Я попытался вывести закон управления ниже:

Допустим, вам нужен ваш ᵂX⁷ ∈ SE (3) (поза связи 7, измеренная и выраженная в мировом фрейме), чтобы соответствовать некоторой желаемой позе ᵂX⁷_des, и вы определяете свою функцию Ляпунова как

V = |ᵂX⁷ - ᵂX⁷_des|_F

, где я использую | |_F для обозначения нормы Фробениуса. Я буду использовать ᵂR⁷ для обозначения матрицы вращения и ᵂp⁷ для обозначения положения ссылки 7 в мировом фрейме.

Эта функция Ляпунова эквивалентна

V = |ᵂR⁷ - ᵂR⁷_des|_F + (ᵂp⁷ - ᵂp⁷_des)ᵀ(ᵂp⁷ - ᵂp⁷_des)
  = trace((ᵂR⁷ - ᵂR⁷_des)ᵀ(ᵂR⁷ - ᵂR⁷_des)) + (ᵂp⁷ - ᵂp⁷_des)ᵀ(ᵂp⁷ - ᵂp⁷_des)
  = 6 - 2*trace(ᵂR⁷_desᵀ * ᵂR⁷) + (ᵂp⁷ - ᵂp⁷_des)ᵀ(ᵂp⁷ - ᵂp⁷_des)

Ваш цель - вычислить градиент ∂V / ∂θ. Чтобы вычислить эту частную производную, вам потребуются частные производные ∂ᵂR⁷ / ∂θ и ∂ᵂp⁷ / ∂θ. Когда вы вычисляете якобиан из CalcJacobianSpatialVelocity, трансляционная часть этого якобиана равна ∂⁷pᵂ / ∂θ (кстати, я больше привык к express позиции / скорости в мировом фрейме, а не в фрейме связи 7), но вращательная часть этого Якобиан - это , а не ∂ᵂR⁷ / ∂θ. Предположим, вы назвали

jacobian = robot.CalcJacobianSpatialVelocity(
    context=differential_ik.GetRobotContext(), with_respect_to=JacobianWrtVariable.kQDot,
    frame_B=robot.GetFrameByName("iiwa_link_7"),
    p_BP=[0.0, 0.0, 0.0],
    frame_A=robot.world_frame(),
    frame_E=robot.world_frame()) 

, вращательная часть этого якобиана равна ∂ ᵂω⁷/∂ θ_dot, где ᵂω⁷ - это angular скорость звена 7, измеренная и выраженная в мировой системе отсчета. Чтобы вычислить ∂ᵂR⁷ / ∂θ, мы используем следующее уравнение

∂ᵂR⁷ / ∂θ = ∂ᵂR_dot⁷ / ∂θ_dot
          = ∂(⌊ᵂω⁷⌋ₓ ᵂR⁷) / ∂θ_dot
          = (∂⌊ᵂω⁷⌋ₓ / ∂θ_dot ) * ᵂR⁷

в первой строке уравнения, я взял производную по времени как числителя, так и знаменателя. Во второй строке уравнения я использую уравнение ᵂR_dot⁷ = ⌊ᵂω⁷⌋ₓ ᵂR⁷, где ⌊ᵂω⁷⌋ₓ представляет собой кососимметричную c матрицу angular скорости ᵂω⁷ (Кстати, я забыл, это ᵂR_dot⁷ = ⌊ᵂω⁷⌋ₓ ᵂR⁷ или ᵂR_dot⁷ = ᵂR⁷⌊ᵂω⁷⌋ₓ. Если один не работает, вы можете попробовать другой) Последняя строка уравнения выполняется, если взять градиент продукта и ∂ᵂR⁷ / ∂θ_dot = 0.