Intergrated GO_HOME state, verified minimal height for setTargetPose