torso integrated with joint_state_listener and working