Загрузите гуманоидную модель робота в память и создайте KinematicsSolver объект для модели. Объект содержит кинематическое представление модели и списка всех объединенных переменных, которые это содержит.

mdl = 'sm_import_humanoid_urdf';
load_system(mdl);
fk = simscape.multibody.KinematicsSolver(mdl);

Добавьте к объекту, fk, группа переменных системы координат для левого запястья. Задайте систему координат B left_wrist соединитесь как последователь и мировая система координат как основа. Назовите группу переменной системы координат Wrist. Объект теперь имеет шесть переменных системы координат — три для x, y, и z компонентов перевода и три для x, y, и z компонентов вращения.

base = 'sm_import_humanoid_urdf/World/W';
follower = 'sm_import_humanoid_urdf/left_wrist/B';
addFrameVariables(fk,'Wrist','translation',base,follower);
addFrameVariables(fk,'Wrist','rotation',base,follower);

Используйте jointPositionVariables(fk) перечислять все объединенные переменные. Присвоение как цели объединенные переменные для колена (j2. Rz.q), лобное плечо (j6. Rz.q), и стреловидное плечо (j7. Rz.q).

jointPositionVariables(fk)
targetIDs = ["j2.Rz.q";"j6.Rz.q";"j7.Rz.q"];
addTargetVariables(fk,targetIDs);

Используйте frameVariables(fk) перечислять все переменные системы координат и присваивать их в Wrist группа как выходные параметры.

frameVariables(fk)
outputIDs = ["Wrist.Translation.x";"Wrist.Translation.y";...
"Wrist.Translation.z";"Wrist.Rotation.x";"Wrist.Rotation.y";"Wrist.Rotation.z"];
addOutputVariables(fk,outputIDs);

Решите прямую задачу кинематики, учитывая колено, лобное плечо, и плечо стреловидные углы поворота шарнира 30, 45, и 45 степени.

targets = [30,45,45];
[outputVec,statusFlag] = solve(fk,targets)

outputVec =

    0.2196
    0.0584
   -0.0983
  135.0000
    0.0027
  -15.0000

statusFlag =

     1

solve функция возвращает значения выходных переменных. Значения сортируются в том же порядке как выходные переменные. Модули являются значениями по умолчанию m для компонентов перевода и deg для компонентов вращения. statusFlag показывает, что всем ограничениям модели и целевым переменным удовлетворяют.

Просмотрите решение.

viewSolution(fk);

Закройте средство просмотра.

closeViewer(fk);

Загрузите гуманоидную модель робота в память и создайте KinematicsSolver объект для модели. Объект содержит кинематическое представление модели и списка всех объединенных переменных, которые это содержит.

mdl = 'sm_import_humanoid_urdf';
load_system(mdl);
ik = simscape.multibody.KinematicsSolver(mdl);

Добавьте к объекту, ik, группа переменных системы координат для правого запястья. Задайте систему координат B right_wrist соединитесь как последователь и мировая система координат как основа. Назовите группу переменной системы координат Wrist. Объект теперь имеет три переменные системы координат для x, y, и z компоненты перевода.

base = 'sm_import_humanoid_urdf/World/W';
follower = 'sm_import_humanoid_urdf/right_wrist/B';
addFrameVariables(ik,'Wrist','translation',base,follower);

Используйте frameVariables(ik) перечислять все переменные системы координат и присваивать их в Wrist группа как цели.

frameVariables(ik)
targetIDs = ["Wrist.Translation.x";"Wrist.Translation.y";"Wrist.Translation.z"];
addTargetVariables(ik,targetIDs);

Используйте jointPositionVariables(ik) перечислять все объединенные переменные и присвоение, как выводит объединенные переменные для колена (j10. Rz.q), лобное плечо (j14. Rz.q), и стреловидное плечо (j15. Rz.q).

jointPositionVariables(ik)
outputIDs = ["j10.Rz.q";"j14.Rz.q";"j15.Rz.q"];
addOutputVariables(ik,outputIDs);

Вычислите углы поворота шарнира для колена и плеча, соответствующего положению запястья [-0.16,-0.12,0] m.

targets = [-0.16,-0.12,0];
[outputVec,statusFlag] = solve(ik,targets)

solve функция возвращает значения выходных переменных — углы поворота колена, лобное плечо, и стреловидное плечо, каждый в модулях по умолчанию deg. statusFlag показывает, что всем ограничениям модели и целевым переменным удовлетворяют.

outputVec =

  -52.8384
  -71.6077
  172.9586


statusFlag =

     1

Визуализируйте решение в Средстве просмотра Решателя Кинематики и определите, разумно ли это.

viewSolution(ik);

Нажмите кнопку Front View на панели инструментов, чтобы просмотреть результат.

Правое запястье находится в нужной области кадра, но правая рука имеет неестественное положение. Обратите внимание на то, что это решение является одним из возможных решений для этой проблемы. Можно задать соединения плеча как переменные предположения, чтобы иметь лучшее решение.

Установите лобное плечо и плечо стреловидные объединенные переменные как переменные предположения и запустите анализ еще раз для вращений [90,90] deg.

guessesIDs=["j14.Rz.q","j15.Rz.q"];
guesses = [90,90];
addInitialGuessVariables(ik,guessesIDs);
[outputVec,statusFlag] = solve(ik,targets,guesses)

solve функция возвращает новое решение для углов поворота шарнира.

outputVec =

  -52.8384
  108.3891
   55.5025

statusFlag =

     1

Нажмите кнопку Update Visualization, чтобы обновить Средство просмотра Решателя Кинематики, чтобы визуализировать результаты.

Закройте средство просмотра.

closeViewer(ik);