Update joint_controller.cpp