Modified NonlinearProgramWConstraintObjects to accept multiple arguments.  Bug fixes to working implicit trajectory optimization, added joint limit forces.