Merge branch 'mujoco-cpp-sensors' of github.com:Caltech-AMBER/obelisk…

… into mujoco-cpp-sensors
Caltech-AMBER · Jul 23, 2024 · 3c5d910 · 3c5d910
2 parents 945ee78 + 00b35fe
commit 3c5d910
Show file tree

Hide file tree

Showing 5 changed files with 59 additions and 12 deletions.
diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h
@@ -7,6 +7,8 @@
 #include <GLFW/glfw3.h>
 #include <mujoco/mujoco.h>
 
+#include "ament_index_cpp/get_package_share_directory.hpp"
+
 #include "obelisk_sensor_msgs/msg/obk_joint_encoders.hpp"
 #include "obelisk_sim_robot.h"
 
@@ -38,14 +40,18 @@ namespace obelisk {
             auto mujoco_config_map     = this->ParseConfigStr(mujoco_setting);
 
             // Get config params
-            xml_path_ = GetXMLPath(mujoco_config_map); // Required
+            xml_path_             = GetXMLPath(mujoco_config_map);      // Required
+            std::string robot_pkg = GetRobotPackage(mujoco_config_map); // Optional
             // Search for the model
             if (!std::filesystem::exists(xml_path_)) {
-                if (const char* obelisk_root = std::getenv("OBELISK_ROOT")) {
-                    xml_path_ = "models" / xml_path_;
-                    xml_path_ = static_cast<std::string>(obelisk_root) / xml_path_;
+                if (robot_pkg != "None" && robot_pkg != "none") {
+                    std::string share_directory = ament_index_cpp::get_package_share_directory(robot_pkg);
+                    xml_path_                   = "mujoco" / xml_path_;
+                    xml_path_                   = share_directory / xml_path_;
                 } else {
-                    throw std::runtime_error("OBELISK_ROOT environment variable not set. Run the dev_setup.sh script!");
+                    RCLCPP_ERROR_STREAM(this->get_logger(),
+                                        "Provided Mujoco XML is NOT an absolute path and robot_pkg is None or not "
+                                        "specified. Please provide a valid Mujoco XML path.");
                 }
             }
 
@@ -57,12 +63,7 @@ namespace obelisk {
 
             configuration_complete_ = true;
 
-            // Setup the sensors
-            // try {
             ParseSensorString(mujoco_config_map.at("sensor_settings"));
-            // } catch (const std::exception& e) {
-            // RCLCPP_WARN_STREAM(this->get_logger(), "Mujoco simulation initialized without any sensors.");
-            // }
 
             return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
         }
@@ -303,6 +304,14 @@ namespace obelisk {
             }
         }
 
+        std::string GetRobotPackage(const std::map<std::string, std::string>& config_map) {
+            try {
+                return config_map.at("robot_pkg");
+            } catch (const std::exception& e) {
+                return "None";
+            }
+        }
+
         int GetNumInputs(const std::map<std::string, std::string>& config_map) {
             try {
                 int nu = std::stoi(config_map.at("n_u"));

diff --git a/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml
@@ -75,10 +75,11 @@ onboard:
           non_obelisk: False
       sim:
         - ros_parameter: mujoco_setting
-          model_xml_path: dummy/dummy.xml
           n_u: 1
           time_step: 0.002
           num_steps_per_viz: 5
+          robot_pkg: dummy_bot
+          model_xml_path: dummy.xml
           sensor_settings:
           - topic: /obelisk/dummy/sensor
             dt: 0.001

diff --git a/obelisk_ws/src/robots/dummy_bot/CMakeLists.txt b/obelisk_ws/src/robots/dummy_bot/CMakeLists.txt
@@ -14,6 +14,12 @@ install(
   DESTINATION share/${PROJECT_NAME}/urdf
 )
 
+# install the mujoco files
+install(
+  DIRECTORY mujoco/
+  DESTINATION share/${PROJECT_NAME}/mujoco
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   set(ament_cmake_copyright_FOUND TRUE)

diff --git a/obelisk_ws/src/robots/dummy_bot/mujoco/common.xml b/obelisk_ws/src/robots/dummy_bot/mujoco/common.xml
@@ -0,0 +1,31 @@
+<mujoco>
+
+  <visual>
+    <headlight ambient=".4 .4 .4" diffuse=".8 .8 .8" specular="0.1 0.1 0.1"/>
+    <map znear=".01"/>
+    <quality shadowsize="2048"/>
+    <global elevation="-15"/>
+  </visual>
+
+  <asset>
+    <texture name="blue_grid" type="2d" builtin="checker" rgb1=".02 .14 .44" rgb2=".27 .55 1" width="300" height="300" mark="edge" markrgb="1 1 1"/>
+    <material name="blue_grid" texture="blue_grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
+
+    <texture name="grey_grid" type="2d" builtin="checker" rgb1=".26 .26 .26" rgb2=".6 .6 .6" width="300" height="300" mark="edge" markrgb="1 1 1"/>
+    <material name="grey_grid" texture="grey_grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
+    <texture name="skybox" type="skybox" builtin="gradient" rgb1=".66 .79 1" rgb2=".9 .91 .93" width="800" height="800"/>
+
+    <material name="self" rgba=".7 .5 .3 1"/>
+    <material name="self_default" rgba=".7 .5 .3 1"/>
+    <material name="self_highlight" rgba="0 .5 .3 1"/>
+    <material name="effector" rgba=".7 .4 .2 1"/>
+    <material name="effector_default" rgba=".7 .4 .2 1"/>
+    <material name="effector_highlight" rgba="0 .5 .3 1"/>
+    <material name="decoration" rgba=".2 .6 .3 1"/>
+    <material name="eye" rgba="0 .2 1 1"/>
+    <material name="target" rgba=".6 .3 .3 1"/>
+    <material name="target_default" rgba=".6 .3 .3 1"/>
+    <material name="target_highlight" rgba=".6 .3 .3 .4"/>
+    <material name="site" rgba=".5 .5 .5 .3"/>
+  </asset>
+</mujoco>
diff --git a/models/dummy/dummy.xml → ..._ws/src/robots/dummy_bot/mujoco/dummy.xml b/models/dummy/dummy.xml → ..._ws/src/robots/dummy_bot/mujoco/dummy.xml
@@ -1,5 +1,5 @@
     <mujoco model="dummy">
-    <include file="../common.xml" />
+    <include file="common.xml" />
 
     <compiler angle="radian" coordinate="local"/>
     <option timestep="0.001" gravity="0 0 -9.81"/>