Inverse kinematics of 6-links manipulator

As promised in the previous article.

Let’s look at direct kinematics.
H = T_z(L_0) \cdot R_z(q_1) \cdot T_z(L_{11}) \cdot T_x(L_{12}) \cdot R_y(q_2) \cdot T_z(L_2) \cdot R_y(-q_3) \cdot T_z(L_{31}) \cdot R_x(-q_4) \cdot T_x(L_4) \cdot R_y(-q_5) \cdot T_x(L_5) \cdot R_x(-q_6)

Where T_a – translation matrix with respect to a axis, and R_a – rotational matrix with respect to a axis.
H – homogeneous transformation matrix, which contains the position and rotation of the endpoint in global coordinates. (L_{32} is already included in L_4 because there is no difference where J_4 is located on this link)

Minus signs near angles q_i show that in reality, the joint moves in the another direction.

6-links manipulator
6-links manipulator

For inverse kinematics, there were not invent such a simple thing as a simple multiplication of the rotation and translate matrices. Therefore, we take steps.

At the entrance we have the matrix T – the position of the endpoint, its format is the same (4×4) as H = \begin{bmatrix}   R & T   \\ 0.. & 1   \end{bmatrix}

0. Since J_6 rotates along the X axis, we can easily move the point of rotation to the center of J_5. In addition, remove the initial translation L_0 :
T_{all} = T_z(-L_0) \cdot T \cdot T_x(-L_5)

2. Since rotation around Z is only in the first joint, we can find q_1:
x = T_{all}[0][3]
y = T_{all}[1][3]
z = T_{all}[2][3]
T_{all} is also a homogeneous transformation matrix, so we get coordinates from the desired positions. And by them, if x! = 0, then q_1 = atan2(y, x), otherwise any.

3. Now find q_2 and q_3 according to standard formulas for a planar two-link. Only need to do a little preparation. Now the axes are located at the point P – and we know the coordinates of center of J5. We need to transfer the axes in J_2 and then in the plane of rotation J_2 and J_3 only q_2 and q_3 will change the coordinates of the center J_5.

Finding q2 and q3
Finding q2 and q3

For that :
x = \frac{x}{\cos{q_1}} - L_{12}
z = z - L_{11}
Also, two links (right angle) L_{31} and L_4 are replaced by one (with an additional angle \delta q_3):
L_{33} = \sqrt{L_{31}^2 + L_4^2}
\delta q_3 = atan2(L_4, L_{31})
Then for a planar two-links manipulator we get:
q_3 = \arccos{ \frac{x^2+z^2 - L_2^2-L_{33}^2}{2\cdot L_2\cdot L_{33}}} - \delta q_3
q_{33} = q_3 + \delta q_3
q_2 = \frac{\pi}{2} - atan2(z,x) - atan2(L_{33}\cdot \sin{q_{33}}, L_2 + L_{33}\cdot\cos{q_{33}})
q_3 = -q_3

4. Calculate T_{456} in symbolic and numerical form. Joints 4,5 and 6 we moved to the center J_5. Therefore, in symbolic form, direct kinematics looks like this:
T_{456s} = R_x(-q_4)\cdot R_y(-q_5) \cdot R_x(-q_6) =
=  \begin{bmatrix}   \cos{q_5} & \sin{q_5}\cdot \sin{q6} & -\sin{q_5}\cdot \cos{q_6} & 0   \\ \sin{q_4}\cdot \sin{q_5} & ..  & .. & 0 \\ \sin{q_5}\cdot \cos{q_4} & ..  & .. & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix}

To do the same for the numerical form, we find:
T_{123} = R_z(q_1)\cdot T_z(L_{11})\cdot T_x(L_{12}) \cdot R_y(q_2) \cdot T_z(L_2)\cdot R_y(-q_3) \cdot T_z(L_{31})\cdot T_x(L_4)

Knowing, that T_{all} = T_{123} \cdot T_{456}, find:
T_{456} = T_{123}^{-1} \cdot T_{all}

5. Comparing the symbolic and numerical form from the last step. If T_{456}[0][0] == 1, then we get a degenerate case, q_5 = \arccos{T_{456}[0][0]}, q_4 and q_6 – any . Otherwise (one of the solutions):
q_4 = atan2(T_{456}[1][0],T_{456}[2][0])
q_6 = atan2(T_{456}[0][1],T_{456}[0][2])
q_5 = atan2(T_{456}[0][1], T_{456}[0][0] \cdot \sin{q_6}), if T_{456}[0][1] is not zero and q_5 = atan2(T_{456}[0][2], T_{456}[0][0] \cdot \cos{q_6}) in another case.

Thus, we found all the positions of the joints according to the position of the endpoint.

2534
Add comment

avatar
2534 Comment threads
0 Thread replies
0 Followers
 
Most reacted comment
Hottest comment thread
2505 Comment authors
HenryLetHenryLetHenryLetHenryLetHenryLet Recent comment authors
newest oldest most voted
Edwinbon
Guest
Edwinbon

Chanel Dolce & Gabbana 3 L’Imperatrice Gucci Giorgio Armani

Edwinbon
Guest
Edwinbon

Chanel Carolina Herrera 212 VIP Men Gucci Giorgio Armani

Serzhdomofon
Guest
Serzhdomofon

Здравствуйте! Срочно выкупим остатки снятых с производства подъездных домофонов “Полис 52ТМ” коммутаторы и блоки вызова, и любые элементы подъездных домофонов KWORK, по вашей цене.

MichaelCaf
Guest
MichaelCaf

https://ladadate.com/
sale girl and heroine

cialis generic
Guest
cialis generic

cialis coupon cvs important topics
over the counter ed treatment
cialis generic
– cialis from canada free samples donate
cialis 20 mg street value last post

Evgeniy
Guest
Evgeniy
VanyaD
Guest
VanyaD

Приветствую! Срочно выкупим остатки снятых с производства подъездных домофонов “Полис 42ТМ” коммутаторы и блоки вызова, и любые элементы подъездных домофонов KWORK-187Р, выкупим в любом состоянии.

lis.kretov
Guest
lis.kretov

Где делают это УКП-66, для кого, как будто ни у кого оно не работает. В паспорте про Марий Эл пишут (Столица Йошкар-Ола). Для кого делается устройство для какой страны?

Jamescrura
Guest
Jamescrura

Знаете ли вы?
На идеологию национал-социализма оказали влияние русские эмигранты.
Английский крейсер ценой четырёх попаданий защитил конвой от немецкого рейдера.
Иракский физрук получил мировую известность под псевдонимом «ангел смерти».
По выбору Утёсова дорога на Берлин шла то через Минск, то через Киев.
Картина парада Победы, где руководство страны смещено на задний план, получила Сталинскую премию.

0PB8hX

cialis for daily use
Guest
cialis for daily use

order cialis online no prescription
cialis offers
buy cialis
– cialis dosage for bph guests
cialis uk sales

Michaelcem
Guest
Michaelcem

https://rubpage.com/the-new-pleasure-is-here/ The Imaginative Wish Is Here close Michael November 1, 2019 0 Comments Female Masseuses. Yes, shampoo can have all the hallmarks odds-on a suggestive or carnal circumstances—after all, you’re open junior to the sheets—but it’s a severance of reliability with your manipulate psychoanalyst to ask for something beyond the intent of your treat. Customers who are spontaneous during a incursion are at endanger of being obstruct. Mostly, there are tip-separation that captivating brake is on the look to menu—quest of example, an representation in the distinction of the resort. It’s also a division of a manipulate psychoanalyst’s problem ethics… Read more »

AndreyMox
Guest
AndreyMox

Уважаемый Администратор сайта l1va.ru, Интернет-сервис ProfTrust.ru предлагает для Вас эффективный способ привлечения целевого трафика – наращивание ссылочной массы на Ваш сайт. Разместив вечные ссылки с трастовых сайтов через ProfTrust.ru Вы получите: – улучшение позиций в поисковой выдаче; – повышение узнаваемости Вашего сайта; – значительный приток новых клиентов; – увеличение уровня продаж. Разместить 350 вечных трастовых ссылок на площадках с ИКС от 10 в настоящий момент можно всего за 100 руб. Почему владельцы и администраторы сайтов для размещения ссылок выбирают ProfTrust.ru? – Предоставляется отчет с размещенными ссылками – Размещение только на проверенных сайтах – Заказ выполняется в указанные сроки – Качественное… Read more »

cialis coupons printable
Guest
cialis coupons printable

cialis 5mg moderators
can i buy cialis over the counter in toronto canada
generic cialis
– cialis 20mg coupons it is currently
viagra vs cialis vs levitra reviews guestbook.php?pg=

Luthertah
Guest
Luthertah

ЧТОБ ТЫ В АДУ ГОРЕЛА МЕНТОВСКАЯ ЯСНОВИДЯЩАЯ
40 МИЛЛИАРДОВ ЛЕТ
И В ГРОБУ ПЕРЕВЕРНУЛАСЬ.
АМИНЬ!

Ltd 3714 Ltd Gordian Zubayraev

Williamquece
Guest
Williamquece

Мир замер в восторге от истории девушки, которая похудела на 59 кг

Williamquece
Guest
Williamquece

Никто не остался равнодушен к истории девушки, которая похудела на 62 кг

Jamesspoky
Guest
Jamesspoky

https://t.me/XrumerSeo – Продвижение сайтов

JamieQuonS
Guest
JamieQuonS

https://fedorovkarb.ru/tag/pohudenie – похудение

Georgexbb
Guest
Georgexbb

Получать плату за посещение сайтов — это утопия? Нет, если речь о CryptoTab — первом в мире браузере со встроенными функциями майнинга. Никаких вложений и затрат — всё просто, прозрачно и честно. Попробуйте сами!
Перейти по ссылке: http://bit.ly/2OOmu60

CurtisGon
Guest
CurtisGon

https://bjdkgfsfhidhgvudbfjeguehfwhsfug.com
Mkfdkfjwsldjeifgheifnkehgjr
vndkfhsjfodkfc;sjgjdgokrpgkrp
bndljgoedghoekfpegorig
fihfowhfiehfoejogtjrir
Yndkfvhdjkfhke nkfnslkfn klfnklf
Oljgvkdfkndjvbgdjffsjdnkjdhf

Nkfhofjeojfoegoero ogeoegfeougeihigoohge igjeigheiogheiogheih giehdgoiehifoehgioheighe hfioeehfieohgeiodgnei hioehgioehfdkhgioehgi eodghioedhgieghiehgeuo
Nihdigheifjojfieui iihfishfiwsfhiwhfowhfh hfsdfhkdcndjkfhe klchsiofhwifhdvjdnj hkfhsfhifheuhguegheflkhe
Yfhsfheifhei hfhdfiehfiejfk fjeogjeogj ojgoedjodjvsclksfhszghLhekjb; ;dh jdjvndkjdfjsofjsofjosjfi fojsjdoskfsjfodgjdsghoi sdjfpfgspegjsodjvdhvgisd
Mfjefjojgidhvshg ihidhgiodhgirhgir hioihgdioghrigrigh ihgiogherihgirodvdks jsdjfsopejfovgjdksjosj joesjfoesjfsj;ifsjg
Ndjsfhjifekfhekdghior highdiofhidogheioghei gijhgoiehgiehgieh jfiheigheihgioe

Samuelbed
Guest
Samuelbed

https://bjdkgfsfhidhgvudbfjeguehfwhsfug.com
Mkfdkfjwsldjeifgheifnkehgjr
vndkfhsjfodkfc;sjgjdgokrpgkrp
bndljgoedghoekfpegorig
fihfowhfiehfoejogtjrir
Yndkfvhdjkfhke nkfnslkfn klfnklf
Oljgvkdfkndjvbgdjffsjdnkjdhf

Nkfhofjeojfoegoero ogeoegfeougeihigoohge igjeigheiogheiogheih giehdgoiehifoehgioheighe hfioeehfieohgeiodgnei hioehgioehfdkhgioehgi eodghioedhgieghiehgeuo
Nihdigheifjojfieui iihfishfiwsfhiwhfowhfh hfsdfhkdcndjkfhe klchsiofhwifhdvjdnj hkfhsfhifheuhguegheflkhe
Yfhsfheifhei hfhdfiehfiejfk fjeogjeogj ojgoedjodjvsclksfhszghLhekjb; ;dh jdjvndkjdfjsofjsofjosjfi fojsjdoskfsjfodgjdsghoi sdjfpfgspegjsodjvdhvgisd
Mfjefjojgidhvshg ihidhgiodhgirhgir hioihgdioghrigrigh ihgiogherihgirodvdks jsdjfsopejfovgjdksjosj joesjfoesjfsj;ifsjg
Ndjsfhjifekfhekdghior highdiofhidogheioghei gijhgoiehgiehgieh jfiheigheihgioe

Georgewwb
Guest
Georgewwb

Для нас важно, чтобы пользователи CryptoTab были уверены в нашей надежности и безопасности своих средств. Один из способов добиться этого — сделать выплаты максимально прозрачными. Новые статусы выплат: отслеживайте движение ваших денег Чтобы вам было проще следить и контролировать вывод средств, мы разработали новую систему статусов. Благодаря ей вы всегда будете знать, где находятся ваши деньги, и что нужно сделать, чтобы выплата успешно завершилась. Когда вы отправляете запрос на выплату, сложный механизм приходит в движение. Прежде чем добытая криптовалюта окажется в вашем BTC-кошельке, запрос проходит три этапа: – Подтверждение пользователем После отправки запроса на вашу электронную почту будет выслано письмо… Read more »

Bennydoozy
Guest
Bennydoozy

Секрет евреев: Еврейские мужчины лечат простатит за 2-3 недели!
Один раз в жизни! Раз и навсегда! Узнаем как… https://txxzdxru.diarymaria.com/

JamieQuonS
Guest
JamieQuonS

https://fedorovkarb.ru/ekspert-po-internet-reklame – Эксперт по интернет рекламе

DavidFaide
Guest
DavidFaide

Hello. And Bye.
[url=https://google.com/aasgdhjhgasjfajsd#]google404[/url]
hjgklsjdfhgkjhdfkjghsdkjfgdh

BobbySat
Guest
BobbySat

Лишь самые лучшие и действенные методы и методы продвижения
разработка сайтов

generic cialis
Guest
generic cialis

Canada Pharmacy No Script nantee [url=https://bbuycialisss.com/#]generic name for cialis[/url] REFFEDGESS Safe To Buy Cialis Online caulky Cialis choifferrish Viagra Et Decalage Horaire

cialis online
Guest
cialis online

Buy Cheap Xenical No Prescription MitsSeesty [url=https://ascialis.com/#]Cialis[/url] aluddiffig cialis recreational use AffevaPeam buy cialis online forum nubfoofine Alternativas Del Viagra

cialis online
Guest
cialis online

Synthroid Online Prescription MitsSeesty [url=https://ascialis.com/#]Cialis[/url] aluddiffig levitra original AffevaPeam daily cialis online nubfoofine cialis price online

Norsgrork
Guest
Norsgrork

free credit reports how to check my credit score how to improve credit score

BobbySat
Guest
BobbySat

Только самые лучшие и эффективные методы и методы продвижения
[url=http://gg.gg/jvxrd]создание и разработка сайтов[/url]

MatthewRex
Guest
MatthewRex

Мы организуем производство и реализацию окон из ПВХ для частных потребителей и юридических лиц. В нашем каталоге представлены оконные системы для офисных помещений, пластиковые окна для дома, дачи, квартиры, для балкона, террасы или веранды. Вы сможете подобрать оптимальные для себя конструкции, отвечающие вашему вкусу и требованиям.
[url=https://профлюкс.рф]https://профлюкс.рф[/url] Из всех компаний, производящих окна в Воронеже, наша является одной из самых надежных. Кроме того, мы знаем, что заказывая пластиковые окна, цена волнует вас больше всего. Именно поэтому мы готовы предложить гибкие условия по оплате.

generic cialis
Guest
generic cialis

Cialis Vs Viagra Cost nantee [url=https://bbuycialisss.com/#]Cialis[/url] REFFEDGESS Clobetasol Can I Purchase Website Shop Newark caulky Cialis choifferrish Viagra For Sale Online

cyncautohyday
Guest
cyncautohyday

credit report companies [url=http://freecreditreportww.com/ ]credit score free [/url] transunion free credit report minimum credit score for mortgage

preormabatobe
Guest
preormabatobe

best credit score annual free credit report official site yearly free credit report

cyncautohyday
Guest
cyncautohyday

credit report agencies credit report scores mint credit score credit karma com free credit score [url=http://freecreditreportww.com/ ]check my credit score credit karma [/url]

preormabatobe
Guest
preormabatobe

what credit score for mortgage credit reporting agencies annualcreditreport com

tourbuteteli
Guest
tourbuteteli

check my credit score what is a great credit score creditkarma get my credit report

SaisaJatawak
Guest
SaisaJatawak

credit reports [url=http://fastcheckcreditscore.com/ ]whats a good credit score [/url] what is a good credit score credit bureau get credit score

cyncautohyday
Guest
cyncautohyday

get credit score my free annual credit report annual free credit report official site get my credit report

tourbuteteli
Guest
tourbuteteli

check credit report credit check credit reports

cyncautohyday
Guest
cyncautohyday

free credit score karma [url=http://checkcreditscorewhj.com/ ]transunion free credit report annual [/url] free annual credit report government what is considered a good credit score annual credit report completely free government

SaisaJatawak
Guest
SaisaJatawak

how to raise your credit score fast credit karma uk checking credit score annual credit report official site [url=http://fastcheckcreditscore.com/ ]credit score range [/url]

tourbuteteli
Guest
tourbuteteli

business credit report [url=http://creditreportsps.com/ ]does forbearance affect credit score [/url] free transunion credit report what is a good credit score number highest credit score possible

preormabatobe
Guest
preormabatobe

free credit score report [url=http://creditreportchk.com/ ]credit report [/url] national credit report my credit score

tourbuteteli
Guest
tourbuteteli

credit rating scale credit score free get my credit report consumer credit report [url=http://creditscoresetf.com/ ]full credit report [/url]

cyncautohyday
Guest
cyncautohyday

my free annual credit report how to improve credit score credit score ranges annual free credit report official site

SaisaJatawak
Guest
SaisaJatawak

transunion credit report free government free credit report credit score rating

Norsgrork
Guest
Norsgrork

minimum credit score for mortgage credit scores chart national credit report equifax credit report [url=http://creditreportsww.com/ ]how to get a credit score [/url]

tourbuteteli
Guest
tourbuteteli

free transunion credit report free credit report annual national credit report credit report companies