Remove accessing YAML node and casting in the control loop.

[pangtao22]

Based on the profiling results shown above, some of the most time-consuming operations in our control loop (the callback function for IIWA_STATUS) are accessing, decoding and casting fields in the configuration YAML node. The Step function only gets two samples whereas the YAML functions combined get more than 100. It might be worthwhile to decode the necessary fields once in the constructor and store the values as private fields of IiwaPlanManager.