MPC GPS

[thobotics]

This PR implement MPC Guided Policy Search, which described in [1].

Main contribution:

1. gps_main.py is modified to make the agent able to run the MPC trajectory optimizer during sampling.
2. mpc_traj_opt.py: implement the MPC algorithm to minimize the surrogate cost as in [1]. Note that the policy cost term in surrogate cost will be replaced by the offline trajectory cost if you are using AlgorithmTrajOpt.
3. Create mobile robot agent for experiment in src/gps_agent_pkg.

Experiment [1]:

1. Launch file: mobilerobot_gps.launch with 3 worlds file (set by parameter): hallway.world, hallway_bend.world, one_obstacle.world.
2. Experiment files: turtlebot_hallway_badmm_example: Trying to move the robot in hallway. turtlebot_badmm_example: Trying to move the robot at desired velocity while avoiding obstacle.

Note:

1. Must using this version of stage_ros package (Check here).

[1] Tianhao Zhang, Gregory Kahn, Sergey Levine, Pieter Abbeel. Learning Deep Control Policies for Autonomous Aerial Vehicles with MPC-Guided Policy Search. ICRA 2016.

[cbfinn]

Thanks for the PR thobotics! @TZ2016 will take the first pass at reviewing.

Glancing over the PR, please make sure that you remove commented out code and remove unneeded changes to parts of the code that MPCGPS does not use.

[tcjcxy30]

Hi: @thobotics , your mpc-gps can not compile with the error: /usr/bin/ld: /usr/lib/libvtkWrappingTools-6.2.a(vtkParse.tab.c.o): relocation R_X86_64_32S against '.rodata' can not be used when making a shared object; recompile with -fPIC /usr/lib/libvtkWrappingTools-6.2.a(vtkParse.tab.c.o): error adding symbols: Bad value collect2: error: ld returned 1 exit status CMakeFiles/gps_agent_lib.dir/build.make:2527: recipe for target 'lib/libgps_agent_lib.so' failed make[2]: *** [lib/libgps_agent_lib.so] Error 1 CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/gps_agent_lib.dir/all' failed make[1]: *** [CMakeFiles/gps_agent_lib.dir/all] Error 2 Makefile:127: recipe for target 'all' failed make: *** [all] Error 2

I think the reason is that the libvtkWrappingTools-6.2.a is a static link library so I try to add some code in CMakeLists.txt to fix the -fPIC problem like:

set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC")

but I still get the same error above. I can compile the master branch of gps normally, so how can I fix this compiled error? My vtk 6.2.0 is installed from source and set the build_share_lib to ON but the libvtkWrappingTools-6.2.a is a static link library. Thankyou very much!