obliczanie prędkości w manipulatorach pomocy

czesc mam problem mam opisac prędkośc manipulatora z 6 stopniami swobody mam policzoną juz kinematyke prosta ale nie moge policzyc prędkosci mam to zrobic według tego wzoru V = Atrans(k-1,k)Φtrans(k-1,k)V(k-1)+Htrans(k)q(k) kinematyke mam policzoną według notacji denavita-Hantenberga. Jak ktos wie jak to zrobic albo wytłumaczyc poszczególne operacje ze wzorów to dzieki mój mail to [email protected]