Программа решения прямой задачи кинематики KUKA youBot

На этой странице представлена программа Mathematica, реализующая вычисления по формулам страницы "Решение прямой задачи кинематики KUKA youBot" . Программу можно просто скопировать с экрана компьютера и вставить в файл Mathematica с расширением "nb" . Аргументы функции FK, отвечающие декартовым координатам и углам, должны быть заданы в мм и в радианах соответственно. Результатом обращения к функции FK является таблица, содержащая параметры локации рабочего органа.

С помощью стандартной функции Manipulate системы Mathematica, можно, как показано ниже, построить обращение к FK при переменных обобщённых координатах робота, изменяемых указателем мыши с помощью слайдеров.

Текст программы

FK[XC_, YC_, θ_ , θ1_ , θ2_ , θ3_ , θ4_ , θ5_ ] :=

Module[

(*Задание параметров*)

{h = 151, a1 = 33, d1 = 147, s = "nonsing.", a2 = 155, a3 = 135, a4 = 81, a5 = 90, a6 = 47},

(*Определение абсолютных координат точки E*)

XE = XC +h Cos[θ] + ((a1 + a2) Sin[θ2] + a3 Sin[θ2 + θ3] + (a4 + a5 + a6) Sin[θ2 + θ3 + θ4]) Cos[θ+θ1];

YE = YC + h Sin[θ] +((a1 + a2) Sin[θ2] + a3 Sin[θ2 + θ3] + (a4 + a5 + a6) Sin[θ2 + θ3 + θ4]) Sin[θ+θ1];

ZE = d1 + a2 Cos[θ2] + a3 Cos[θ2 + θ3] + (a4 + a5 + a6) Cos[θ2 + θ3 + θ4];

(*Определение угла нутации θ_E∈ [0,π] базиса Exyz относит. базиса OXYZ*)

θE=ArcTan[Cos[θ2 + θ3 + θ4],Abs[Sin[θ2 + θ3 + θ4]]];

(*Определение суммы ϕ+ψ∈[-π,π] в сингулярных случаях θ2 + θ3 + θ4=0и θ2 + θ3 + θ4=±π*)

If[Abs[θ2 + θ3 + θ4]<=$MinMachineNumber,{s="sing.", ϕPlusψ=ArcTan[Cos[θ+θ1+θ5],Sin[θ+θ1+θ5]]}];

If[Abs[Abs[θ2 + θ3 + θ4] - Pi]<=$MinMachineNumber,{s="sing.", ϕPlusψ=ArcTan[Cos[θ+θ1-θ5+Pi],Sin[θ+θ1-θ5+Pi]]}];

(*Определение углов ψ∈[-π,π],ϕ∈[-π,π] базиса Exyz относит. базиса OXY в несингулярных

случаях θ_E∈ (0,π) *)

If [ s =="nonsing." , If [ θ2 + θ3 + θ4 > Pi || - Pi < θ2 + θ3 + θ4 < 0 , { ψ = ArcTan [ Sin [ θ + θ1 ] , - Cos [ θ + θ1 ] ] , ϕ= ArcTan [ - Sin [ θ5 ] , Cos [ θ5 ] ] } , { ψ = ArcTan [ - Sin [ θ + θ1 ] , Cos [ θ + θ1 ] ] , ϕ = ArcTan [ Sin [ θ5 ] , - Cos [ θ5 ] ] } ] ] ;

(* Преобразование ⁢ отрицат . значений ⁢ углов ⁢ ψ , ϕ , ϕ + ψ ⁢ к положит. на ⁢ интервале [ 0 , 2 ⁢ π ) *)

⁢If [ ϕ < 0 , ϕ = 2 ⁢ Pi + ϕ ] ; ⁢

If [ ψ < 0 , ψ = 2 ⁢ Pi + ψ ] ;

⁢If [ ϕPlusψ < 0 , ϕPlusψ = 2 ⁢ Pi + ϕPlusψ ] ;

⁢(* Формирование ⁢ строк ⁢ таблицы ⁢ результатов в ⁢ сингулярных ⁢ и ⁢ несингулярных ⁢ случаях *)

If [ s == "sing.",

{ string1 = { "sing. or nonsing.", "X_E ","Y_E"," Z_E","ψ + ϕ" , " θE", SpanFromLeft } ;

string2 = { "", "(mm) ","(mm)","(mm)","[0,360), гр." , " [0,180], гр.", SpanFromLeft } ;

string3 = { s , XE , YE , ZE, ϕPlusψ 180 / Pi , θE 180 / Pi , SpanFromLeft } } ,

{ string1 = { "sing. or nonsing.", "X_E","Y_E","Z_E",

"ψ" , " θE", " ϕ" , SpanFromLeft } ;

string2 = { "", "(mm)","(mm)","(mm)","[0,360 ),гр." , "[0,180],гр.",

"[0,360),гр." , SpanFromLeft } ;

string3 = { s , XE , YE , ZE , ψ 180 / Pi , θE 180 / Pi , ϕ 180 / Pi ,

SpanFromLeft } } ] ;

(*Формирование таблицы результатов*)

Return[

Grid[{{"ОБОБЩЁННЫЕ КООРДИНАТЫ ПЛАТФОРМЫ И РУКИ",

SpanFromLeft},

{"XC", "YC",

" θ", " θ1", "θ2", "θ3",

"θ4", "θ5"},

{"(mm)", "(mm)",

"[0,360),гр.", "[-169 ,169],гр.", "[-65,90],гр.", "[-151,146 ],гр.",

" [-102.5,102.5],гр.","[-167.5,167.5],гр."},{XC, YC, θ 180/ Pi, θ1 180/ Pi,

θ2 180/Pi, θ3 180/Pi, θ4 180/ Pi, θ5 180/ Pi},

{"ПАРАМЕТРЫ ЛОКАЦИИ РАБОЧЕГО ОРГАНА ", SpanFromLeft}, {" РЕШЕНИЕ ПРЯМОЙ

ЗАДАЧИ КИНЕМАТИКИ ", SpanFromLeft}, string1, string2, string3},

ItemSize -> {5, 4} , Frame -> All]]]

Текст программы

Manipulate[

FK[XC, YC, θ , θ1 , θ2 , θ3 , θ4 , θ5],