mpflueger · zkytony · Dec 5, 2022 · Dec 5, 2022 · Dec 6, 2022 · Dec 6, 2022
diff --git 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,17 +53,15 @@ 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
 		src/mujoco_ompl_interface.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,17 +73,15 @@ 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
 		src/mujoco_ompl_interface.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
@@ -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:
+
+<img alt="reacher_plan_graph" src="https://user-images.githubusercontent.com/7720184/205773512-b4c8dce1-c341-45c5-ae98-4d956a513514.png" width="300px"/>
+
+
+
+**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:
+
+<img alt="reacher_plan_render" src="https://user-images.githubusercontent.com/7720184/205774142-d90aeb2b-15dc-40bb-b4ef-4560028110f3.png" width="400px"/>
+
+
+[**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:
+
+<img alt="render_plan_collision_avoidance" src="https://user-images.githubusercontent.com/7720184/206159014-f62194c1-0a68-4a12-b715-0c74ed6b7022.png" width="400px"/>
+
diff --git a/include/mujoco_wrapper.h b/include/mujoco_wrapper.h
@@ -7,7 +7,7 @@
 #include <mutex>
 #include <vector>
 
-#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
@@ -22,6 +22,6 @@
 		</body>
 	</worldbody>
 	<actuator>
-		<motor gear="100" joint="slider" name="slide"/>
+		<motor ctrllimited="true" gear="100" joint="slider" name="slide"/>
 	</actuator>
-</mujoco>
+</mujoco>
diff --git a/render_plan.cpp b/render_plan.cpp
@@ -4,7 +4,7 @@
 #include <fstream>
 #include <string>
 
-#include "glfw3.h"
+#include <GLFW/glfw3.h>
 
 #include "mujoco_wrapper.h"
 #include "mujoco_ompl_interface.h"
@@ -159,4 +159,4 @@ int main(int argc, char** argv) {
     mjr_freeContext(&con);
 
     return 0;
-}
+}
diff --git a/render_plan_kinematic.cpp b/render_plan_kinematic.cpp
@@ -4,7 +4,7 @@
 #include <fstream>
 #include <string>
 
-#include "glfw3.h"
+#include <GLFW/glfw3.h>
 
 #include "mujoco_wrapper.h"
 #include "mujoco_ompl_interface.h"

diff --git a/src/mujoco_ompl_interface.cpp b/src/mujoco_ompl_interface.cpp
@@ -228,7 +228,7 @@ shared_ptr<ob::CompoundStateSpace> makeCompoundStateSpace(
             break;
         }
         space->addSubspace(joint_space, 1.0);
-    } 
+    }
     if (next_qpos != m->nq) {
         cerr << "ERROR: joint dims: " << next_qpos
              << " vs nq: " << m->nq << endl;