cartwheel: calculate kinetic and potential energy of robot
cartwheel: pass full and joint space state of robot to controller