diff --git a/CMakeLists.txt b/CMakeLists.txt index 89d05b2..920cf6f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,44 +6,29 @@ set(CMAKE_CXX_FLAGS "-g -Wall -std=c++11") find_package(Eigen3 3.3 REQUIRED NO_MODULE) if(NOT OMPL_INCLUDE_DIRS) - find_package(ompl REQUIRED) + find_package(ompl REQUIRED) endif() -find_package(Threads REQUIRED) -find_package(OpenGL REQUIRED) find_package(GLEW REQUIRED) +find_package(glfw3 REQUIRED) + +find_package(Threads REQUIRED) find_package(yaml-cpp REQUIRED) find_package(Boost 1.58 QUIET REQUIRED COMPONENTS serialization filesystem system program_options) -add_library(mujoco-nogl SHARED IMPORTED) -add_library(mujoco SHARED IMPORTED) -add_library(glfw3 SHARED IMPORTED) - -if(APPLE) - set_target_properties(mujoco-nogl PROPERTIES - IMPORTED_LOCATION "$ENV{HOME}/.mujoco/mujoco200/bin/libmujoco200nogl.dylib" - INTERFACE_INCLUDE_DIRECTORIES "$ENV{HOME}/.mujoco/mujoco200/include" ) - - set_target_properties(mujoco PROPERTIES - IMPORTED_LOCATION "$ENV{HOME}/.mujoco/mujoco200/bin/libmujoco200.dylib" - INTERFACE_INCLUDE_DIRECTORIES "$ENV{HOME}/.mujoco/mujoco200/include" ) - - set_target_properties(glfw3 PROPERTIES - IMPORTED_LOCATION "$ENV{HOME}/.mujoco/mujoco200/bin/libglfw.3.dylib" - INTERFACE_INCLUDE_DIRECTORIES "$ENV{HOME}/.mujoco/mujoco200/include" ) -elseif(UNIX) - set_target_properties(mujoco-nogl PROPERTIES - IMPORTED_LOCATION "$ENV{HOME}/.mujoco/mujoco200/bin/libmujoco200nogl.so" - INTERFACE_INCLUDE_DIRECTORIES "$ENV{HOME}/.mujoco/mujoco200/include" ) - - set_target_properties(mujoco PROPERTIES - IMPORTED_LOCATION "$ENV{HOME}/.mujoco/mujoco200/bin/libmujoco200.so" - INTERFACE_INCLUDE_DIRECTORIES "$ENV{HOME}/.mujoco/mujoco200/include" ) - - set_target_properties(glfw3 PROPERTIES - IMPORTED_LOCATION "$ENV{HOME}/.mujoco/mujoco200/bin/libglfw.so.3" - INTERFACE_INCLUDE_DIRECTORIES "$ENV{HOME}/.mujoco/mujoco200/include" ) -endif() +# Debugging info +message(STATUS "-------- DEBUGGING ------------") +get_cmake_property(_variableNames VARIABLES) +foreach (_variableName ${_variableNames}) + message(STATUS "${_variableName}=${${_variableName}}") +endforeach() +message(STATUS "-------- END DEBUGGING --------") + + +# linking mujoco +include_directories(${CMAKE_SOURCE_DIR}/mujoco-2.3.0/include) +link_directories(${CMAKE_SOURCE_DIR}/mujoco-2.3.0/lib) + include_directories(${OMPL_INCLUDE_DIRS}, ${Boost_INCLUDE_DIR}) include_directories(3rd_party/include) @@ -55,7 +40,7 @@ add_executable(plan plan.cpp ) target_link_libraries(plan ${OMPL_LIBRARIES}) target_link_libraries(plan Eigen3::Eigen) -target_link_libraries(plan mujoco-nogl) +target_link_libraries(plan mujoco) target_link_libraries(plan Threads::Threads) target_link_libraries(plan yaml-cpp) target_link_libraries(plan Boost::filesystem) @@ -68,9 +53,7 @@ target_link_libraries(render_plan ${OMPL_LIBRARIES}) target_link_libraries(render_plan Eigen3::Eigen) target_link_libraries(render_plan mujoco) target_link_libraries(render_plan Threads::Threads) -target_link_libraries(render_plan glfw3) -target_link_libraries(render_plan OpenGL::GL) -target_link_libraries(render_plan GLEW::GLEW) +target_link_libraries(render_plan glfw) add_executable(plan_kinematic plan_kinematic.cpp src/mujoco_wrapper.cpp @@ -78,7 +61,7 @@ add_executable(plan_kinematic plan_kinematic.cpp ) target_link_libraries(plan_kinematic ${OMPL_LIBRARIES}) target_link_libraries(plan_kinematic Eigen3::Eigen) -target_link_libraries(plan_kinematic mujoco-nogl) +target_link_libraries(plan_kinematic mujoco) target_link_libraries(plan_kinematic Threads::Threads) target_link_libraries(plan_kinematic yaml-cpp) @@ -90,9 +73,7 @@ target_link_libraries(render_plan_kinematic ${OMPL_LIBRARIES}) target_link_libraries(render_plan_kinematic Eigen3::Eigen) target_link_libraries(render_plan_kinematic mujoco) target_link_libraries(render_plan_kinematic Threads::Threads) -target_link_libraries(render_plan_kinematic glfw3) -target_link_libraries(render_plan_kinematic OpenGL::GL) -target_link_libraries(render_plan_kinematic GLEW::GLEW) +target_link_libraries(render_plan_kinematic glfw) add_executable(smooth_plan smooth_plan.cpp src/mujoco_wrapper.cpp @@ -100,7 +81,7 @@ add_executable(smooth_plan smooth_plan.cpp ) target_link_libraries(smooth_plan ${OMPL_LIBRARIES}) target_link_libraries(smooth_plan Eigen3::Eigen) -target_link_libraries(smooth_plan mujoco-nogl) +target_link_libraries(smooth_plan mujoco) target_link_libraries(smooth_plan Threads::Threads) target_link_libraries(smooth_plan yaml-cpp) @@ -110,7 +91,7 @@ add_library(mujoco_ompl_nogl SHARED target_include_directories(mujoco_ompl_nogl PUBLIC include) target_link_libraries(mujoco_ompl_nogl ${OMPL_LIBRARIES}) target_link_libraries(mujoco_ompl_nogl Eigen3::Eigen) -target_link_libraries(mujoco_ompl_nogl mujoco-nogl) +target_link_libraries(mujoco_ompl_nogl mujoco) target_link_libraries(mujoco_ompl_nogl Threads::Threads) target_link_libraries(mujoco_ompl_nogl yaml-cpp) @@ -123,6 +104,3 @@ target_link_libraries(mujoco_ompl Eigen3::Eigen) target_link_libraries(mujoco_ompl mujoco) target_link_libraries(mujoco_ompl Threads::Threads) target_link_libraries(mujoco_ompl yaml-cpp) -target_link_libraries(mujoco_ompl glfw3) -target_link_libraries(mujoco_ompl OpenGL::GL) -target_link_libraries(mujoco_ompl GLEW::GLEW) diff --git a/README.md b/README.md index c5789f8..3788d46 100644 --- a/README.md +++ b/README.md @@ -8,44 +8,95 @@ Some of the code found here is mildly project specific, and in the future we may **Contact:** Max Pflueger `pflueger` at `usc.edu` -## Requirements -#### OMPL -Install from here: https://ompl.kavrakilab.org/installation.html -(If you install it from source, make sure to run 'make install'.) -#### MuJoCo -- Download mujoco200 here: https://www.roboti.us/index.html -- Unpack libraries to ~/.mujoco/mujoco200 -- Place license file at ~/.mujoco/mjkey.txt +## Build Instructions (new) -#### Eigen3 -#### OpenGL -#### GLEW +Note: below is currently only tested on Ubuntu 20.04. +1. Download MuJoCo release from https://github.com/deepmind/mujoco/releases/tag/2.3.0 (last tested version: 2.3.0) -## Build +2. Extract the content of compressed MuJoCo release into a folder `mujoco-2.3.0` under this repo's root directory. + +3. Install OMPL following instructions on https://ompl.kavrakilab.org/installation.html (last tested version 1.5.0) + +4. Ensure the following libraries can be found by CMake: ompl, Eigen3, GLEW, GLFW, Threads, yaml-cpp, Boost. + +5. Build: + ``` + mkdir build && cd build + cmake .. + make + ``` + + Example output of `make`: + ``` + [ 11%] Built target mujoco_ompl_nogl + [ 26%] Built target smooth_plan + [ 42%] Built target render_plan_kinematic + [ 57%] Built target plan_kinematic + [ 69%] Built target mujoco_ompl + Scanning dependencies of target render_plan + [ 73%] Building CXX object CMakeFiles/render_plan.dir/render_plan.cpp.o + [ 76%] Building CXX object CMakeFiles/render_plan.dir/src/mujoco_wrapper.cpp.o + [ 80%] Building CXX object CMakeFiles/render_plan.dir/src/mujoco_ompl_interface.cpp.o + [ 84%] Linking CXX executable render_plan + [ 84%] Built target render_plan + Scanning dependencies of target plan + [ 88%] Building CXX object CMakeFiles/plan.dir/plan.cpp.o + [ 92%] Building CXX object CMakeFiles/plan.dir/src/mujoco_wrapper.cpp.o + [ 96%] Building CXX object CMakeFiles/plan.dir/src/mujoco_ompl_interface.cpp.o + [100%] Linking CXX executable plan + [100%] Built target plan + ``` + + +## Run (new) + +Note: below is currently only tested on Ubuntu 20.04. + + +**To create a plan file** (Assume you are in the repo root directory): ``` -cmake . -make +./build/plan ./problems/reacher_prob.yaml -o reacher_plan.out ``` - -## Run -MacOS requires to include the mujoco bin directory in the DYLD_LIBRARY_PATH: +The output of this program ends with ``` -DYLD_LIBRARY_PATH=/Users/$USER/.mujoco/mujoco200/bin +... +Found Solution with status: Exact solution +Solution wrote to file "reacher_sol.out" ``` -To create a plan file: +**To visualize a plan as a graph** (Assume you are in repo root directory): ``` -./plan reacher.xml reacher_prob.yaml 10 +python3 plot_plan.py reacher_sol.out problems/reacher_info.yaml ``` -To visualize a plan as a graph: +It may look like this: + +reacher_plan_graph + + + +**To rollout a plan in MuJoCo with rendering** (Assume you are in repo root directory) ``` -python3 plot_plan.py plan.out reacher_info.yaml +./build/render_plan problems/reacher.xml build/reacher_sol.out ``` -To rollout a plan in MuJoCo with OpenGL rendering: +You may see the reacher spinning in place, like: + +reacher_plan_render + + +[**etpr**](https://github.com/etpr) added kinematic planner and collision check (see [pull request](https://github.com/mpflueger/mujoco-ompl/pull/3)). +You can try it with the 2d_point problem by +``` +./build/plan_kinematic problems/2d_point.xml problems/2d_point_prob.yaml 1 +``` +The plan will be written to `plan.out`. You can render it by ``` -./render_plan reacher.xml plan.out +./build/render_plan_kinematic problems/2d_point.xml ./plan.out ``` +Visualization: + +render_plan_collision_avoidance + diff --git a/include/mujoco_wrapper.h b/include/mujoco_wrapper.h index fee0bc6..9f37729 100644 --- a/include/mujoco_wrapper.h +++ b/include/mujoco_wrapper.h @@ -7,7 +7,7 @@ #include #include -#include "mujoco.h" +#include "mujoco/mujoco.h" struct JointInfo { @@ -91,7 +91,7 @@ class MuJoCo { return d; } - + std::string getJointName(int i) const { // Avert your eyes of this horror @@ -140,19 +140,19 @@ class MuJoCo { d->time = s.time; for(size_t i=0; i < m->nq; i++) { if (i >= s.qpos.size()) break; - d->qpos[i] = s.qpos[i]; + d->qpos[i] = s.qpos[i]; } for(size_t i=0; i < m->nv; i++) { if (i >= s.qvel.size()) break; - d->qvel[i] = s.qvel[i]; + d->qvel[i] = s.qvel[i]; } for(size_t i=0; i < m->na; i++) { if (i >= s.act.size()) break; - d->act[i] = s.act[i]; + d->act[i] = s.act[i]; } for(size_t i=0; i < m->nu; i++) { if (i >= s.ctrl.size()) break; - d->ctrl[i] = s.ctrl[i]; + d->ctrl[i] = s.ctrl[i]; } } @@ -198,4 +198,3 @@ class MuJoCo { static int mj_instance_count; static std::mutex mj_instance_count_lock; }; - diff --git a/problems/cartpole_swingup.xml b/problems/cartpole_swingup.xml index 76e3ca8..6dd04cc 100644 --- a/problems/cartpole_swingup.xml +++ b/problems/cartpole_swingup.xml @@ -22,6 +22,6 @@ - + - \ No newline at end of file + diff --git a/render_plan.cpp b/render_plan.cpp index b6f3457..1877a4a 100644 --- a/render_plan.cpp +++ b/render_plan.cpp @@ -4,7 +4,7 @@ #include #include -#include "glfw3.h" +#include #include "mujoco_wrapper.h" #include "mujoco_ompl_interface.h" @@ -159,4 +159,4 @@ int main(int argc, char** argv) { mjr_freeContext(&con); return 0; -} \ No newline at end of file +} diff --git a/render_plan_kinematic.cpp b/render_plan_kinematic.cpp index 1974132..ff27d51 100644 --- a/render_plan_kinematic.cpp +++ b/render_plan_kinematic.cpp @@ -4,7 +4,7 @@ #include #include -#include "glfw3.h" +#include #include "mujoco_wrapper.h" #include "mujoco_ompl_interface.h" diff --git a/src/mujoco_ompl_interface.cpp b/src/mujoco_ompl_interface.cpp index f67501a..ab7541e 100644 --- a/src/mujoco_ompl_interface.cpp +++ b/src/mujoco_ompl_interface.cpp @@ -228,7 +228,7 @@ shared_ptr makeCompoundStateSpace( break; } space->addSubspace(joint_space, 1.0); - } + } if (next_qpos != m->nq) { cerr << "ERROR: joint dims: " << next_qpos << " vs nq: " << m->nq << endl;