added calculation of robot twist