Working on a balancing controller
Added logic to handle coupled joint in motor identification module