Added Pose Checking. Testing needs to be done