Demonstration of contingency handling using safety constraints and ergonomics handling

Summary
Demonstration of contingency handling using safety constraints and ergonomics handling