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

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 для компонентов вращения. The statusFlag показывает, что все ограничения модели и целевые переменные удовлетворены.

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

viewSolution(fk);

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

closeViewer(fk);

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

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. The statusFlag показывает, что все ограничения модели и целевые переменные удовлетворены.

outputVec =

  -52.8384
  -71.6077
  172.9586


statusFlag =

     1

Визуализируйте решение в Kinematics Solver Viewer и определите, является ли оно разумным.

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, чтобы обновить Kinematics Solver Viewer, чтобы визуализировать результаты.

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

closeViewer(ik);