Kinematyka prosta egzoszkieletu ramienia

[jabbah]

Witam,
konstruuję w ramach pracy magisterskiej system do teleoperacji.

Chciałbym zastosować następującą konstrukcję egzoszkieletu dla operatora:

Żeby odpowiednio przełożyć dane pobierane z enkoderów egzoszkieletu na ruch robota,
muszę rozwiązać zadanie proste kinematyki dla powyższego schematu.

Znalazłem gotowe rozwiązanie, które umieszczam poniżej, jednak nie jestem co do niego przekonany - wykonałem parę własnych obliczeń i żadne wyniki nie pokrywają się z "gotowcem", zgodne są tylko dla pierwszej macierzy transformacji.

Czy ktoś obyty w temacie mógłby na to zerknąć i powiedzieć mi czy to ja popełniam błąd, czy też gotowiec jest zrypany?

Pozdrawiam,
Michał