From 3518cc4013a354cf1877313ad30813ed110a5776 Mon Sep 17 00:00:00 2001 From: Kaiyu Zheng Date: Mon, 5 Dec 2022 18:40:30 -0500 Subject: [PATCH 1/8] able to build with deepmind/mujoco and latest ompl --- CMakeLists.txt | 68 ++++++++++++----------------------- include/mujoco_wrapper.h | 13 ++++--- render_plan.cpp | 4 +-- render_plan_kinematic.cpp | 2 +- src/mujoco_ompl_interface.cpp | 2 +- 5 files changed, 33 insertions(+), 56 deletions(-) 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/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/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; From fbf18393c7dbaf14605fcaa71c1e8584b2da0dc0 Mon Sep 17 00:00:00 2001 From: Kaiyu Zheng Date: Mon, 5 Dec 2022 18:51:27 -0500 Subject: [PATCH 2/8] new README instructions for build --- README.md | 50 ++++++++++++++++++++++++++++++++++---------------- 1 file changed, 34 insertions(+), 16 deletions(-) diff --git a/README.md b/README.md index c5789f8..44c12d5 100644 --- a/README.md +++ b/README.md @@ -8,26 +8,44 @@ 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) +1. Download MuJoCo release from https://github.com/deepmind/mujoco/releases/tag/2.3.0 (last tested version: 2.3.0) -#### Eigen3 -#### OpenGL -#### GLEW +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 + ``` -## Build -``` -cmake . -make -``` ## Run MacOS requires to include the mujoco bin directory in the DYLD_LIBRARY_PATH: From 8a3960c13def8b8fc07574117ac250b5d9d951be Mon Sep 17 00:00:00 2001 From: Kaiyu Zheng Date: Mon, 5 Dec 2022 19:01:52 -0500 Subject: [PATCH 3/8] new readme instructions for running --- README.md | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index 44c12d5..012339f 100644 --- a/README.md +++ b/README.md @@ -10,6 +10,9 @@ Some of the code found here is mildly project specific, and in the future we may ## Build Instructions (new) + +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) 2. Extract the content of compressed MuJoCo release into a folder `mujoco-2.3.0` under this repo's root directory. @@ -47,23 +50,22 @@ Some of the code found here is mildly project specific, and in the future we may ``` -## Run -MacOS requires to include the mujoco bin directory in the DYLD_LIBRARY_PATH: -``` -DYLD_LIBRARY_PATH=/Users/$USER/.mujoco/mujoco200/bin -``` +## Run (new) + +Note: below is currently only tested on Ubuntu 20.04. + -To create a plan file: +To create a plan file (Assume you are in the `build` directory): ``` -./plan reacher.xml reacher_prob.yaml 10 +./plan ../problems/reacher_prob.yaml -o reacher_plan.out ``` -To visualize a plan as a graph: +To visualize a plan as a graph (Assume you are in repo root directory): ``` -python3 plot_plan.py plan.out reacher_info.yaml +python plot_plan.py build/reacher_sol.out problems/reacher_info.yaml ``` -To rollout a plan in MuJoCo with OpenGL rendering: +To rollout a plan in MuJoCo with rendering (Assume you are in repo root directory) ``` -./render_plan reacher.xml plan.out +./build/render_plan problems/reacher.xml build/reacher_sol.out ``` From 46867dd71c387c31444883df93d36564a86a0251 Mon Sep 17 00:00:00 2001 From: Kaiyu Zheng <7720184+zkytony@users.noreply.github.com> Date: Mon, 5 Dec 2022 19:14:20 -0500 Subject: [PATCH 4/8] Update README.md --- README.md | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 012339f..0dc1f8a 100644 --- a/README.md +++ b/README.md @@ -59,13 +59,29 @@ To create a plan file (Assume you are in the `build` directory): ``` ./plan ../problems/reacher_prob.yaml -o reacher_plan.out ``` +The output of this program ends with +``` +... +Found Solution with status: Exact solution +Solution wrote to file "reacher_sol.out" +``` To visualize a plan as a graph (Assume you are in repo root directory): ``` -python plot_plan.py build/reacher_sol.out problems/reacher_info.yaml +python3 plot_plan.py build/reacher_sol.out problems/reacher_info.yaml ``` +It may look like this: + +reacher_plan_graph + + + To rollout a plan in MuJoCo with rendering (Assume you are in repo root directory) ``` ./build/render_plan problems/reacher.xml build/reacher_sol.out ``` + +You may see the reacher spinning in place, like: + +reacher_plan_render From a26331d363955e65ed2cd62219162d6fb0032873 Mon Sep 17 00:00:00 2001 From: Kaiyu Zheng Date: Mon, 5 Dec 2022 19:15:14 -0500 Subject: [PATCH 5/8] minor README clarity --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 0dc1f8a..1a3d318 100644 --- a/README.md +++ b/README.md @@ -55,7 +55,7 @@ Note: below is currently only tested on Ubuntu 20.04. Note: below is currently only tested on Ubuntu 20.04. -To create a plan file (Assume you are in the `build` directory): +**To create a plan file** (Assume you are in the `build` directory): ``` ./plan ../problems/reacher_prob.yaml -o reacher_plan.out ``` @@ -66,7 +66,7 @@ Found Solution with status: Exact solution Solution wrote to file "reacher_sol.out" ``` -To visualize a plan as a graph (Assume you are in repo root directory): +**To visualize a plan as a graph** (Assume you are in repo root directory): ``` python3 plot_plan.py build/reacher_sol.out problems/reacher_info.yaml ``` @@ -77,7 +77,7 @@ It may look like this: -To rollout a plan in MuJoCo with rendering (Assume you are in repo root directory) +**To rollout a plan in MuJoCo with rendering** (Assume you are in repo root directory) ``` ./build/render_plan problems/reacher.xml build/reacher_sol.out ``` From 95c907bcf4c74bdae4e27bf6bf219d49c3bc84c2 Mon Sep 17 00:00:00 2001 From: Kaiyu Zheng Date: Wed, 7 Dec 2022 05:45:49 -0500 Subject: [PATCH 6/8] need ctrllimited=true for cartpole swingup for new mujoco --- problems/cartpole_swingup.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 + From 6e17c12bbbae9bc022ad0d7b249ef7c9490634c3 Mon Sep 17 00:00:00 2001 From: Kaiyu Zheng Date: Wed, 7 Dec 2022 05:46:03 -0500 Subject: [PATCH 7/8] instructions to use plan_kinematic --- README.md | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 1a3d318..9e934b8 100644 --- a/README.md +++ b/README.md @@ -55,9 +55,9 @@ Note: below is currently only tested on Ubuntu 20.04. Note: below is currently only tested on Ubuntu 20.04. -**To create a plan file** (Assume you are in the `build` directory): +**To create a plan file** (Assume you are in the repo root directory): ``` -./plan ../problems/reacher_prob.yaml -o reacher_plan.out +./build/plan ./problems/reacher_prob.yaml -o reacher_plan.out ``` The output of this program ends with ``` @@ -68,7 +68,7 @@ Solution wrote to file "reacher_sol.out" **To visualize a plan as a graph** (Assume you are in repo root directory): ``` -python3 plot_plan.py build/reacher_sol.out problems/reacher_info.yaml +python3 plot_plan.py reacher_sol.out problems/reacher_info.yaml ``` It may look like this: @@ -85,3 +85,15 @@ It may look like this: 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 +``` +./build/render_plan_kinematic problems/2d_point.xml ./plan.out +``` +Visualization: From d61d87dac0ce5b00b0dac6732358457efba5c4b2 Mon Sep 17 00:00:00 2001 From: Kaiyu Zheng <7720184+zkytony@users.noreply.github.com> Date: Wed, 7 Dec 2022 05:47:29 -0500 Subject: [PATCH 8/8] Update README.md --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index 9e934b8..3788d46 100644 --- a/README.md +++ b/README.md @@ -97,3 +97,6 @@ The plan will be written to `plan.out`. You can render it by ./build/render_plan_kinematic problems/2d_point.xml ./plan.out ``` Visualization: + +render_plan_collision_avoidance +