Added basic movement functions

Used inverse holonomic kinematics  matrix