implemented L-BFGS. also, add checker to pr2_sim for data stamps (in case moving and getting last data measurement)