How to calculate position and orientation of top grip.

HapticMaster has 6 degree of freedom.

Top view of HapticMaster. HapticMaster has 3 units.

In each unit,the position of end effector is calculated as follow.

To transform each local position to absolute coordinate position as follow.

For Unit1

For Unit2

For Unit3

The position of top grip is calculated by each position of end effector of each unit.

Position of the grip is expressed by each position of end effector as

We can calculate roll,pitch,yaw using transform matrix.

home.