From 0b34e40b51a7e8d9c1e8995516e49e0879fb6189 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Wed, 28 Jan 2026 20:15:48 -0800 Subject: [PATCH 01/20] wip heightscan --- obelisk/cpp/obelisk_cpp/CMakeLists.txt | 9 +- .../include/obelisk_mujoco_sim_robot.h | 269 ++++++---------- .../obelisk_cpp/lidar/height_scan_interface.h | 165 ++++++++++ .../cpp/obelisk_cpp/lidar/lidar_interface.h | 176 +++++++++++ .../obelisk_cpp/lidar/ray_caster_interface.h | 210 +++++++++++++ .../src/cpp/obelisk_sim_cpp/CMakeLists.txt | 22 ++ .../obelisk_sim_cpp/src/ray_caster_demo.cpp | 96 ++++++ .../obelisk_sensor_msgs/CMakeLists.txt | 1 + .../obelisk_sensor_msgs/msg/ObkScan.msg | 6 + .../src/obelisk_ros/config/g1_scan_cpp.yaml | 293 ++++++++++++++++++ .../config/height_scan_config.yaml | 10 + .../src/obelisk_ros/config/lidar_config.yaml | 12 + .../robots/g1_description/mujoco/g1_27dof.xml | 3 + 13 files changed, 1093 insertions(+), 179 deletions(-) create mode 100644 obelisk/cpp/obelisk_cpp/lidar/height_scan_interface.h create mode 100644 obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h create mode 100644 obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h create mode 100644 obelisk_ws/src/cpp/obelisk_sim_cpp/src/ray_caster_demo.cpp create mode 100644 obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/msg/ObkScan.msg create mode 100644 obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml create mode 100644 obelisk_ws/src/obelisk_ros/config/height_scan_config.yaml create mode 100644 obelisk_ws/src/obelisk_ros/config/lidar_config.yaml diff --git a/obelisk/cpp/obelisk_cpp/CMakeLists.txt b/obelisk/cpp/obelisk_cpp/CMakeLists.txt index 2aca96ff..cfc81360 100644 --- a/obelisk/cpp/obelisk_cpp/CMakeLists.txt +++ b/obelisk/cpp/obelisk_cpp/CMakeLists.txt @@ -34,6 +34,9 @@ option(USE_MUJOCO "Enable Mujoco support" ${USE_MUJOCO_DEFAULT}) message(STATUS "USE_MUJOCO option set to: ${USE_MUJOCO}") if(USE_MUJOCO) + find_package(Eigen3 REQUIRED) + find_package(yaml-cpp REQUIRED) + include(FetchContent) set(MUJOCO_VERSION "3.1.6" CACHE STRING "mujoco version") set(COMP_ARCH "x86_64" CACHE STRING "computer architecture") @@ -61,7 +64,7 @@ endif() # ------- Source files ------- # message(STATUS "cmake source dir: ${CMAKE_CURRENT_SOURCE_DIR}") set(CORE_INC "${CMAKE_CURRENT_SOURCE_DIR}/include") - +set(LIDAR_INC "${CMAKE_CURRENT_SOURCE_DIR}/lidar") # These are not currently used, but might be used in the future # set(OBELISK_CPP_HEADER_LIST # "${CORE_INC}/obelisk_node.h" @@ -79,8 +82,8 @@ target_include_directories(ObkCore INTERFACE ${CORE_INC}) # Conditionally add Mujoco dependencies if(USE_MUJOCO) - target_include_directories(ObkCore INTERFACE ${mujoco_SOURCE_DIR}/include) - target_link_libraries(ObkCore INTERFACE mujoco-lib glfw) + target_include_directories(ObkCore INTERFACE ${mujoco_SOURCE_DIR}/include ${LIDAR_INC}) + target_link_libraries(ObkCore INTERFACE mujoco-lib glfw Eigen3::Eigen yaml-cpp) # Define compile definition for conditional compilation target_compile_definitions(ObkCore INTERFACE OBELISK_USE_MUJOCO) diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h index 0c483163..8c6b413a 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h @@ -21,7 +21,10 @@ #include "ament_index_cpp/get_package_share_directory.hpp" #include "obelisk_sensor_msgs/msg/obk_joint_encoders.hpp" +#include "obelisk_sensor_msgs/msg/obk_scan.hpp" #include "obelisk_sim_robot.h" +#include "height_scan_interface.h" +#include "lidar_interface.h" namespace obelisk { template class ObeliskMujocoRobot : public ObeliskSimRobot { @@ -74,13 +77,6 @@ namespace obelisk { num_steps_per_viz_ = GetNumStepsPerViz(mujoco_config_map); configuration_complete_ = true; - - this->declare_parameter("height_map_geom_group", std::vector{0}); - this->declare_parameter("height_map_grid_size", std::vector{0.}); - this->declare_parameter("height_map_grid_spacing", 0.0); - height_map_grid_size_ = this->get_parameter("height_map_grid_size").as_double_array(); - height_map_grid_spacing_ = this->get_parameter("height_map_grid_spacing").as_double(); - height_map_geom_group_ = this->get_parameter("height_map_geom_group").as_integer_array(); ParseSensorString(mujoco_config_map.at("sensor_settings")); @@ -478,6 +474,12 @@ namespace obelisk { throw std::runtime_error("No sensor type provided for a sensor!"); } + // Check for config path + std::string sensor_config_path = ""; + if (auto it = setting_map.find("config_path"); it != setting_map.end()) { + sensor_config_path = it->second; + } + std::vector sensor_names; std::vector mj_sensor_types; const std::string name_delim = "&"; @@ -558,18 +560,48 @@ namespace obelisk { sensor_names, mj_sensor_types, this->template GetPublisher(sensor_key)), callback_group_); - } else if (sensor_type == "GridCells") { + } else if (sensor_type == "ObkScan") { // Make a publisher and add it to the list - auto pub = ObeliskNode::create_publisher(topic, depth); + auto pub = ObeliskNode::create_publisher(topic, depth); this->publishers_[sensor_key] = - std::make_shared>(pub); + std::make_shared>(pub); + + // Create the scanner interface + // Parse the yaml + YAML::Node scan_config = YAML::LoadFile(sensor_config_path); + const auto type_node = scan_config["pattern"]["type"]; + if (!type_node) { + throw std::runtime_error("scan config missing pattern.type"); + } + + const std::string type_str = type_node.as(); + if (type_str == "height_scan") { + scan_interface_ = std::make_unique(sensor_config_path); + } else if (type_str == "lidar_scan") { + scan_interface_ = std::make_unique(sensor_config_path); + } else { + RCLCPP_ERROR_STREAM( + this->get_logger(), + "Provided RayCaster Scan type " + type_str + "is invalid. Valid types are 'height_scan' and 'lidar_scan'" + ); + } + + scan_dots_idx_ = scn.ngeom; + scn.ngeom += scan_interface_->get_num_rays(); + mjtNum size[3] = {0.01, 0, 0}; // sphere uses size[0] + float rgba[4] = {1, 0, 0, 1}; // red + for (int ii = 0; ii < scan_interface_->get_num_rays(); ++ii) { + mjvGeom g; + mjv_initGeom(&g, mjGEOM_SPHERE, size, nullptr, nullptr, rgba); + scn.geoms[scan_dots_idx_ + ii] = g; + } // Add the timer to the list this->timers_[sensor_key] = this->create_wall_timer( std::chrono::milliseconds(static_cast(1e3 * dt)), - CreateTimerCallback( + CreateTimerCallback( sensor_names, mj_sensor_types, - this->template GetPublisher(sensor_key)), + this->template GetPublisher(sensor_key)), callback_group_); } else { throw std::runtime_error("Sensor type not supported!"); @@ -1130,181 +1162,67 @@ namespace obelisk { RCLCPP_INFO_STREAM(this->get_logger(), "Timer callback created for a Odometry!"); return cb; - } else if constexpr (std::is_same::value) { + } else if constexpr (std::is_same::value) { // ------------------------------------------ // // ------------ Scan Dots Sensor ------------ // // ------------------------------------------ // auto cb = [publisher, sensor_names, mj_sensor_types, this]() { // This sensor is always made up of: // - Framepos + std::lock_guard lock(sensor_data_mut_); - nav_msgs::msg::GridCells msg; - - bool has_framepos = false; - - // Verify that the proper parameters have been passed - if (this->height_map_grid_size_.size() != 2) { - throw std::runtime_error("Attempted to use a scan dots sensor without providing a valid grid size!"); + obelisk_sensor_msgs::msg::ObkScan msg; + std::string site = scan_interface_->get_site(); + int site_id = mj_name2id(model_, mjOBJ_SITE, site.c_str()); + if (site_id == -1) { + throw std::runtime_error("Sensor not found in Mujoco! Make sure your XML has the site: " + scan_interface_->get_site()); } - int x_rays = this->height_map_grid_size_[0] / this->height_map_grid_spacing_ + 1; - int y_rays = this->height_map_grid_size_[1] / this->height_map_grid_spacing_ + 1; - msg.cells.resize(x_rays * y_rays); - if (this->height_map_grid_spacing_ == 0) { - throw std::runtime_error("Attempted to use a scan dots sensor without providing a valid grid spacing!"); + // Starting ray origin position (top-left corner of scan) + // position (world) + Eigen::Vector3d pos(data_->site_xpos[3*site_id + 0], data_->site_xpos[3*site_id + 1], data_->site_xpos[3*site_id + 2]); + Eigen::Matrix3d rot; + rot << data_->site_xmat[9*site_id + 0], data_->site_xmat[9*site_id + 1], data_->site_xmat[9*site_id + 2], + data_->site_xmat[9*site_id + 3], data_->site_xmat[9*site_id + 4], data_->site_xmat[9*site_id + 5], + data_->site_xmat[9*site_id + 6], data_->site_xmat[9*site_id + 7], data_->site_xmat[9*site_id + 8]; + + // Temp storage for geom id output + int geom_id[1] = { -1 }; + + obelisk::RayCasterInterface::MatrixX3d starts_w, dirs_w; + starts_w.resize(scan_interface_->get_num_rays(), 3); + dirs_w.resize(scan_interface_->get_num_rays(), 3); + + scan_interface_->compute_rays_world(rot, pos, starts_w, dirs_w); + + for (int ii = 0; ii < scan_interface_->get_num_rays(); ++ii) { + Eigen::Vector3d ray_origin = starts_w.row(ii).transpose(); + Eigen::Vector3d direction = dirs_w.row(ii).transpose(); + + // Perform ray cast + double dist = mj_ray(this->model_, this->data_, ray_origin.data(), direction.data(), scan_interface_->get_geom_group_mask(), 1, -1, geom_id); + + // Compute hit point + std::array hit_point = { + ray_origin[0] + direction[0] * dist, + ray_origin[1] + direction[1] * dist, + ray_origin[2] + direction[2] * dist + }; + float ret = scan_interface_->get_return(hit_point, dist); + msg.data.push_back(ret); + mjvGeom& g = scn.geoms[scan_dots_idx_ + ii]; + g.type = mjGEOM_SPHERE; + g.pos[0] = hit_point[0]; g.pos[1] = hit_point[1]; g.pos[2] = hit_point[2]; + g.size[0] = 0.01; + g.rgba[0] = 1.f; g.rgba[1] = 0.f; g.rgba[2] = 0.f; g.rgba[3] = 1.f; } - - std::array x_y_num_rays = {x_rays, y_rays}; - - msg.cell_width = this->height_map_grid_spacing_; - - std::lock_guard lock(sensor_data_mut_); - for (size_t i = 0; i < sensor_names.size(); i++) { - int sensor_id = mj_name2id(this->model_, mjOBJ_SENSOR, sensor_names.at(i).c_str()); - if (sensor_id == -1) { - throw std::runtime_error("Sensor not found in Mujoco! Make sure your XML has the sensor: " + sensor_names.at(i)); - } - // Get the sensor id - int sensor_addr = this->model_->sensor_adr[sensor_id]; - - if (mj_sensor_types.at(i) == "framepos") { - if (!has_framepos) { - // Starting ray origin position (top-left corner of scan) - std::array sensor_pos = {this->data_->sensordata[sensor_addr], - this->data_->sensordata[sensor_addr + 1], - this->data_->sensordata[sensor_addr + 2] + 10.0 }; // shift upward - - int site_id = model_->sensor_objid[sensor_id]; - std::array site_pos_global = {this->data_->site_xpos[3*site_id], - this->data_->site_xpos[3*site_id + 1], - this->data_->site_xpos[3*site_id + 2], - }; - - sensor_pos[0] += site_pos_global[0]; - sensor_pos[1] += site_pos_global[1]; - - // Get the yaw from the quat - double w = this->data_->qpos[3], x = this->data_->qpos[4], y = this->data_->qpos[5], z = this->data_->qpos[6]; - - // Yaw (Z-axis rotation) - double siny_cosp = 2.0 * (w * z + x * y); - double cosy_cosp = 1.0 - 2.0 * (y * y + z * z); - double base_yaw = std::atan2(siny_cosp, cosy_cosp); - - // Make the rotation matrix - double cos_yaw = std::cos(base_yaw); - double sin_yaw = std::sin(base_yaw); - double R_2d[2][2] = { - { cos_yaw, -sin_yaw }, - { sin_yaw, cos_yaw } - }; - - // Adjust X and Y to go to bottom-left corner - std::array offset = { -this->height_map_grid_size_[0] / 2.0, -this->height_map_grid_size_[1] / 2.0 }; - // Rotate offset: R_2d x offset - std::array rotated_offset = { - R_2d[0][0] * offset[0] + R_2d[0][1] * offset[1], - R_2d[1][0] * offset[0] + R_2d[1][1] * offset[1] - }; - sensor_pos[0] += rotated_offset[0]; - sensor_pos[1] += rotated_offset[1]; - - // Ray direction: straight down - const std::array direction = { 0.0, 0.0, -1.0 }; - - // Geom groups active for ray collisions - mjtByte geom_group[mjNGROUP] = {0, 0, 0, 0, 0, 0}; - for (size_t i = 0; i < this->height_map_geom_group_.size(); i++) { - geom_group[this->height_map_geom_group_[i]] = 1; - } - - // Temp storage for geom id output - int geom_id[1] = { -1 }; - - int ii = 0; - for (int x = 0; x < x_y_num_rays[0]; ++x) { - for (int y = 0; y < x_y_num_rays[1]; ++y) { - // Compute origin for this ray - offset = { this->height_map_grid_spacing_ * x, this->height_map_grid_spacing_ * y}; - - // Rotate offset: R_2d x offset - rotated_offset = { - R_2d[0][0] * offset[0] + R_2d[0][1] * offset[1], - R_2d[1][0] * offset[0] + R_2d[1][1] * offset[1] - }; - - std::array ray_origin = { - sensor_pos[0] + rotated_offset[0], - sensor_pos[1] + rotated_offset[1], - sensor_pos[2] // z already offset upward - }; - - // Perform ray cast - double dist = mj_ray(this->model_, this->data_, ray_origin.data(), direction.data(), geom_group, 1, -1, geom_id); - - // Compute hit point - std::array hit_point = { - ray_origin[0] + direction[0] * dist, - ray_origin[1] + direction[1] * dist, - ray_origin[2] + direction[2] * dist - }; - geometry_msgs::msg::Point ros_pt; - // Making it relative to the base link - ros_pt.x = hit_point[0] - site_pos_global[0]; - ros_pt.y = hit_point[1] - site_pos_global[1]; - ros_pt.z = hit_point[2]; - msg.cells[ii] = ros_pt; - ii++; - } - } - - if (this->model_->sensor_refid[sensor_id] == -1) { - msg.header.frame_id = "world"; - } else { - int ref_type = this->model_->sensor_reftype[sensor_id]; - int ref_id = this->model_->sensor_refid[sensor_id]; - if (ref_type == mjOBJ_SITE) { - msg.header.frame_id = - std::string(this->model_->names + this->model_->name_siteadr[ref_id]); - } else if (ref_type == mjOBJ_BODY) { - msg.header.frame_id = - std::string(this->model_->names + this->model_->name_bodyadr[ref_id]); - } else if (ref_type == mjOBJ_GEOM) { - msg.header.frame_id = - std::string(this->model_->names + this->model_->name_geomadr[ref_id]); - } else if (ref_type == mjOBJ_CAMERA) { - msg.header.frame_id = - std::string(this->model_->names + this->model_->name_camadr[ref_id]); - } else { - RCLCPP_ERROR_STREAM(this->get_logger(), - "Framepos sensor, " - << sensor_names.at(i) - << ", is not associated with a supported Mujoco object " - "type! Current object type (mjtObj): " - << ref_type); - } - } - has_framepos = true; - } else { - RCLCPP_ERROR_STREAM(this->get_logger(), - "There are two framepos associated with this FramePose! Ignoring " - << sensor_names.at(i)); - } - } else { - RCLCPP_ERROR_STREAM( - this->get_logger(), - "Sensor " << sensor_names.at(i) - << " is not associated with a valid Mujoco sensor type! Current sensor type: " - << mj_sensor_types.at(i)); - } - - msg.header.stamp = this->now(); - } + msg.header.frame_id = "world"; + msg.header.stamp = this->now(); publisher->publish(msg); }; - RCLCPP_INFO_STREAM(this->get_logger(), "Timer callback created for a Scan Dots (GridCells) sensor!"); + RCLCPP_INFO_STREAM(this->get_logger(), "Timer callback created for a Scan Dots (ObkScan) sensor!"); return cb; } } @@ -1473,9 +1391,8 @@ namespace obelisk { int num_sensors_; // For the height map - std::vector height_map_grid_size_; - double height_map_grid_spacing_; - std::vector height_map_geom_group_; + std::unique_ptr scan_interface_; + int scan_dots_idx_; // Constants static constexpr float TIME_STEP_DEFAULT = 0.002; diff --git a/obelisk/cpp/obelisk_cpp/lidar/height_scan_interface.h b/obelisk/cpp/obelisk_cpp/lidar/height_scan_interface.h new file mode 100644 index 00000000..062c1bb8 --- /dev/null +++ b/obelisk/cpp/obelisk_cpp/lidar/height_scan_interface.h @@ -0,0 +1,165 @@ +#pragma once + +#include "ray_caster_interface.h" + +namespace obelisk { + +/** + * @brief Height scan ray caster using a grid pattern + * + * Creates a 2D grid of rays pointing in a fixed direction (typically downward). + * Uses yaw-only rotation for the ray starts, but does NOT rotate ray directions. + * This is suitable for terrain height mapping on legged robots. + * + * YAML Config: + * site_name: "height_scan_site" # MuJoCo site name (required) + * geom_groups: [0] # Geom groups for collision (optional) + * pattern: + * ordering: "xy" # Grid ordering: "xy" or "yx" (optional, default: "xy") + * resolution: 0.05 # Grid step size in meters (required) + * size: [1.0, 1.0] # [width, height] in meters (required) + * direction: [0, 0, -1] # Ray direction in local frame (optional, default: down) + * offset: # Offset transform applied in local frame (optional) + * pos: [0, 0, 0] # Position offset [x, y, z] + * rot: [1, 0, 0, 0] # Rotation quaternion (w, x, y, z) + */ +class HeightScanInterface : public RayCasterInterface { + public: + explicit HeightScanInterface(const std::string& yaml_path) : RayCasterInterface(yaml_path) { + setup_rays(get_config()); + } + + void compute_rays_world(const Matrix3d& site_xmat, const Vector3d& site_pos, + MatrixX3d& ray_starts_world, + MatrixX3d& ray_directions_world) const override { + // Extract yaw from rotation matrix + // site_xmat is column-major in Eigen: R(row, col) + // yaw = atan2(R[1,0], R[0,0]) + double yaw = std::atan2(site_xmat(1, 0), site_xmat(0, 0)); + double c = std::cos(yaw); + double s = std::sin(yaw); + + // Yaw-only rotation matrix (rotation about Z axis) + Matrix3d Rz; + Rz << c, -s, 0.0, + s, c, 0.0, + 0.0, 0.0, 1.0; + + // Transform ray starts: Rz * starts^T, then transpose back to Nx3 + ray_starts_world = (Rz * ray_starts_local_.transpose()).transpose(); + ray_starts_world.rowwise() += site_pos.transpose(); + + // Directions are NOT rotated for height scan (always point in fixed world direction) + ray_directions_world = ray_directions_local_; + } + + protected: + void setup_rays(const YAML::Node& config) override { + auto pattern = config["pattern"]; + if (!pattern) { + throw std::runtime_error("HeightScan config missing 'pattern' section"); + } + + // Read ordering: "xy" or "yx" (affects meshgrid indexing) + std::string ordering = "xy"; + if (pattern["ordering"]) { + ordering = pattern["ordering"].as(); + if (ordering != "xy" && ordering != "yx") { + throw std::runtime_error("ordering must be 'xy' or 'yx', got: " + ordering); + } + } + + // Read resolution (step size in meters) + if (!pattern["resolution"]) { + throw std::runtime_error("HeightScan config missing 'resolution'"); + } + double resolution = pattern["resolution"].as(); + if (resolution <= 0) { + throw std::runtime_error("resolution must be > 0"); + } + + // Read size [width, height] in meters + if (!pattern["size"]) { + throw std::runtime_error("HeightScan config missing 'size'"); + } + auto size_vec = pattern["size"].as>(); + if (size_vec.size() != 2) { + throw std::runtime_error("size must have exactly 2 elements [width, height]"); + } + double width = size_vec[0]; + double height = size_vec[1]; + + // Read direction (default: downward) + Vector3d direction(0.0, 0.0, -1.0); + if (pattern["direction"]) { + auto dir_vec = pattern["direction"].as>(); + if (dir_vec.size() == 3) { + direction = Vector3d(dir_vec[0], dir_vec[1], dir_vec[2]); + double norm = direction.norm(); + if (norm > 1e-9) { + direction /= norm; + } else { + throw std::runtime_error("direction must have positive norm"); + } + } + } + + // Generate grid points + // x: from -width/2 to +width/2, step = resolution + // y: from -height/2 to +height/2, step = resolution + std::vector x_vals, y_vals; + for (double x = -width / 2.0; x <= width / 2.0 + 1e-9; x += resolution) { + x_vals.push_back(x); + } + for (double y = -height / 2.0; y <= height / 2.0 + 1e-9; y += resolution) { + y_vals.push_back(y); + } + + int nx = static_cast(x_vals.size()); + int ny = static_cast(y_vals.size()); + int num_rays = nx * ny; + + ray_starts_local_.resize(num_rays, 3); + ray_directions_local_.resize(num_rays, 3); + + // Create meshgrid and flatten + // ordering == "xy": numpy indexing='xy', outer loop over y, inner over x + // ordering == "yx": numpy indexing='ij', outer loop over x, inner over y + int idx = 0; + if (ordering == "xy") { + for (int j = 0; j < ny; ++j) { + for (int i = 0; i < nx; ++i) { + ray_starts_local_(idx, 0) = x_vals[i]; + ray_starts_local_(idx, 1) = y_vals[j]; + ray_starts_local_(idx, 2) = 0.0; + idx++; + } + } + } else { // "yx" (or "ij" indexing) + for (int i = 0; i < nx; ++i) { + for (int j = 0; j < ny; ++j) { + ray_starts_local_(idx, 0) = x_vals[i]; + ray_starts_local_(idx, 1) = y_vals[j]; + ray_starts_local_(idx, 2) = 0.0; + idx++; + } + } + } + + // All rays have the same direction + for (int i = 0; i < num_rays; ++i) { + ray_directions_local_.row(i) = direction.transpose(); + } + + // Apply offset transform (rotation to directions, position to starts) + Offset offset = parse_offset(pattern); + apply_offset(offset); + } + + float get_return(const std::array hit_point, const float dist) override { + (void)dist; + return float(hit_point[2]); + } +}; + +} // namespace obelisk diff --git a/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h b/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h new file mode 100644 index 00000000..a220ae04 --- /dev/null +++ b/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h @@ -0,0 +1,176 @@ +#pragma once + +#include "ray_caster_interface.h" + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +namespace obelisk { + +/** + * @brief LIDAR ray caster using a spherical pattern + * + * Creates rays in a spherical pattern based on vertical/horizontal FOV. + * Uses full 3D rotation for both ray starts and directions. + * Suitable for simulating 3D LIDAR sensors like Velodyne, Ouster, etc. + * + * YAML Config: + * site_name: "lidar_site" # MuJoCo site name (required) + * geom_groups: [0] # Geom groups for collision (optional) + * pattern: + * vertical_fov_range: [-15, 15] # [min, max] degrees (required) + * horizontal_fov_range: [0, 360] # [min, max] degrees (required) + * channels: 16 # Number of vertical channels (required) + * horizontal_res: 1.0 # Degrees per horizontal step (required) + * offset: # Offset transform applied in local frame (optional) + * pos: [0, 0, 0] # Position offset [x, y, z] + * rot: [1, 0, 0, 0] # Rotation quaternion (w, x, y, z) + */ +class LidarInterface : public RayCasterInterface { + public: + explicit LidarInterface(const std::string& yaml_path) : RayCasterInterface(yaml_path) { + setup_rays(get_config()); + } + + void compute_rays_world(const Matrix3d& site_xmat, const Vector3d& site_pos, + MatrixX3d& ray_starts_world, + MatrixX3d& ray_directions_world) const override { + // Full 3D rotation for both starts and directions + // ray_starts_local_ is Nx3, site_xmat is 3x3 + // world = R * local^T, then transpose back to Nx3 + + ray_starts_world = (site_xmat * ray_starts_local_.transpose()).transpose(); + ray_starts_world.rowwise() += site_pos.transpose(); + + ray_directions_world = (site_xmat * ray_directions_local_.transpose()).transpose(); + } + + protected: + void setup_rays(const YAML::Node& config) override { + auto pattern = config["pattern"]; + if (!pattern) { + throw std::runtime_error("Lidar config missing 'pattern' section"); + } + + // Read vertical FOV range [min_deg, max_deg] + if (!pattern["vertical_fov_range"]) { + throw std::runtime_error("Lidar config missing 'vertical_fov_range'"); + } + auto vert_fov = pattern["vertical_fov_range"].as>(); + if (vert_fov.size() != 2) { + throw std::runtime_error("vertical_fov_range must have 2 elements [min, max]"); + } + double vert_min_deg = vert_fov[0]; + double vert_max_deg = vert_fov[1]; + + // Read horizontal FOV range [min_deg, max_deg] + if (!pattern["horizontal_fov_range"]) { + throw std::runtime_error("Lidar config missing 'horizontal_fov_range'"); + } + auto horz_fov = pattern["horizontal_fov_range"].as>(); + if (horz_fov.size() != 2) { + throw std::runtime_error("horizontal_fov_range must have 2 elements [min, max]"); + } + double horz_min_deg = horz_fov[0]; + double horz_max_deg = horz_fov[1]; + + // Read number of vertical channels + if (!pattern["channels"]) { + throw std::runtime_error("Lidar config missing 'channels'"); + } + int channels = pattern["channels"].as(); + if (channels <= 0) { + throw std::runtime_error("channels must be > 0"); + } + + // Read horizontal resolution (degrees per step) + if (!pattern["horizontal_res"]) { + throw std::runtime_error("Lidar config missing 'horizontal_res'"); + } + double horz_res_deg = pattern["horizontal_res"].as(); + if (horz_res_deg <= 0) { + throw std::runtime_error("horizontal_res must be > 0"); + } + + // Compute vertical angles (in radians) + std::vector vert_angles_rad; + if (channels == 1) { + vert_angles_rad.push_back(vert_min_deg * M_PI / 180.0); + } else { + for (int i = 0; i < channels; ++i) { + double angle_deg = + vert_min_deg + i * (vert_max_deg - vert_min_deg) / (channels - 1); + vert_angles_rad.push_back(angle_deg * M_PI / 180.0); + } + } + + // Compute horizontal angles + double horz_span = horz_max_deg - horz_min_deg; + int num_horizontal = + static_cast(std::ceil(horz_span / horz_res_deg)) + 1; // +1 for endpoints + + // Handle 360-degree wraparound: exclude last point to avoid overlap + bool is_full_rotation = std::abs(std::abs(horz_span) - 360.0) < 1e-6; + + std::vector horz_angles_rad; + for (int i = 0; i < num_horizontal; ++i) { + // Skip last point for full rotation to avoid duplicate at 0/360 + if (is_full_rotation && i == num_horizontal - 1) { + continue; + } + double angle_deg; + if (num_horizontal == 1) { + angle_deg = horz_min_deg; + } else { + angle_deg = horz_min_deg + i * (horz_max_deg - horz_min_deg) / (num_horizontal - 1); + } + horz_angles_rad.push_back(angle_deg * M_PI / 180.0); + } + + // Create meshgrid and compute ray directions (spherical to Cartesian) + int nv = static_cast(vert_angles_rad.size()); + int nh = static_cast(horz_angles_rad.size()); + int num_rays = nv * nh; + + ray_starts_local_.resize(num_rays, 3); + ray_directions_local_.resize(num_rays, 3); + + // All rays start from origin in local frame + ray_starts_local_.setZero(); + + // Meshgrid with indexing="ij" (outer: vertical, inner: horizontal) + int idx = 0; + for (int vi = 0; vi < nv; ++vi) { + double v_angle = vert_angles_rad[vi]; + double cv = std::cos(v_angle); + double sv = std::sin(v_angle); + + for (int hi = 0; hi < nh; ++hi) { + double h_angle = horz_angles_rad[hi]; + double ch = std::cos(h_angle); + double sh = std::sin(h_angle); + + // Spherical to Cartesian (Z up convention) + // x = cos(elevation) * cos(azimuth) + // y = cos(elevation) * sin(azimuth) + // z = sin(elevation) + ray_directions_local_(idx, 0) = cv * ch; + ray_directions_local_(idx, 1) = cv * sh; + ray_directions_local_(idx, 2) = sv; + idx++; + } + } + + // Apply offset transform (rotation to directions, position to starts) + Offset offset = parse_offset(pattern); + apply_offset(offset); + } + + float get_return(const std::array hit_point, const float dist) override { + (void)hit_point; + return dist; + } +}; + +} // namespace obelisk diff --git a/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h b/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h new file mode 100644 index 00000000..b2d28e39 --- /dev/null +++ b/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h @@ -0,0 +1,210 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace obelisk { + +/** + * @brief Apply quaternion rotation to a vector + * @param quat Quaternion (w, x, y, z) + * @param vec 3D vector to rotate + * @return Rotated vector + */ +inline Eigen::Vector3d quat_apply(const Eigen::Quaterniond& quat, const Eigen::Vector3d& vec) { + return quat * vec; +} + +/** + * @brief Abstract base class for ray casting sensors (LIDAR, height scan, etc.) + * + * This interface provides a common structure for computing ray origins and directions + * in both local and world frames. Concrete implementations define the ray pattern + * (grid, spherical, etc.) and transformation behavior. + * + * Usage: + * 1. Construct with path to YAML config file + * 2. Call compute_rays_world() with site pose to get world-frame rays + * 3. Use rays with mj_ray() for raycasting + */ +class RayCasterInterface { + public: + using Matrix3d = Eigen::Matrix3d; + using Vector3d = Eigen::Vector3d; + using MatrixX3d = Eigen::Matrix; + + /** + * @brief Construct from YAML config file path + * @param yaml_path Path to YAML configuration file + * @throws std::runtime_error if file not found or missing required fields + */ + explicit RayCasterInterface(const std::string& yaml_path) { + if (!std::filesystem::exists(yaml_path)) { + throw std::runtime_error("Ray caster config file not found: " + yaml_path); + } + + config_ = YAML::LoadFile(yaml_path); + + // Read site name (required) + if (config_["site_name"]) { + site_name_ = config_["site_name"].as(); + } else { + throw std::runtime_error("site_name is required in ray caster config"); + } + + // Read geom groups for mj_ray filtering (optional, default: group 0 only) + std::fill(std::begin(geom_group_mask_), std::end(geom_group_mask_), 0); + if (config_["geom_groups"]) { + auto groups = config_["geom_groups"].as>(); + for (int g : groups) { + if (g >= 0 && g < mjNGROUP) { + geom_group_mask_[g] = 1; + } + } + } else { + geom_group_mask_[0] = 1; + } + } + + virtual ~RayCasterInterface() = default; + + // Disable copy (YAML::Node has complex copy semantics) + RayCasterInterface(const RayCasterInterface&) = delete; + RayCasterInterface& operator=(const RayCasterInterface&) = delete; + + // Allow move + RayCasterInterface(RayCasterInterface&&) = default; + RayCasterInterface& operator=(RayCasterInterface&&) = default; + + /** + * @brief Get the MuJoCo site name for ray origin reference + */ + const std::string& get_site() const { return site_name_; } + + /** + * @brief Get number of rays in the pattern + */ + int get_num_rays() const { return static_cast(ray_starts_local_.rows()); } + + /** + * @brief Get ray starts in local frame (read-only) + */ + const MatrixX3d& get_ray_starts_local() const { return ray_starts_local_; } + + /** + * @brief Get ray directions in local frame (read-only) + */ + const MatrixX3d& get_ray_directions_local() const { return ray_directions_local_; } + + /** + * @brief Get geom group mask for mj_ray + */ + const mjtByte* get_geom_group_mask() const { return geom_group_mask_; } + + /** + * @brief Compute ray starts and directions in world frame + * @param site_xmat 3x3 rotation matrix from site orientation + * @param site_pos 3D position of site in world frame + * @param[out] ray_starts_world Output matrix of ray start positions (Nx3) + * @param[out] ray_directions_world Output matrix of ray directions (Nx3) + */ + virtual void compute_rays_world(const Matrix3d& site_xmat, const Vector3d& site_pos, + MatrixX3d& ray_starts_world, + MatrixX3d& ray_directions_world) const = 0; + + /** + * @brief Construct the proper return type for the ray caster + * @param hit_point The hit point in 3D + * @param dist The distance to the hit point + */ + virtual float get_return(const std::array hit_point, const float dist) = 0; + + protected: + /** + * @brief Offset transform (position and optional rotation) + */ + struct Offset { + Vector3d pos = Vector3d::Zero(); + Eigen::Quaterniond rot = Eigen::Quaterniond::Identity(); // (w, x, y, z) + }; + + /** + * @brief Setup ray pattern from YAML config (called by derived class constructor) + * @param config YAML node containing pattern-specific configuration + */ + virtual void setup_rays(const YAML::Node& config) = 0; + + /** + * @brief Get the loaded config for derived classes + */ + const YAML::Node& get_config() const { return config_; } + + /** + * @brief Parse offset from YAML pattern config + * @param pattern YAML node containing the pattern section + * @return Offset struct with position and rotation + */ + Offset parse_offset(const YAML::Node& pattern) const { + Offset offset; + + if (pattern["offset"]) { + auto offset_node = pattern["offset"]; + + // Read position offset + if (offset_node["pos"]) { + auto pos_vec = offset_node["pos"].as>(); + if (pos_vec.size() == 3) { + offset.pos = Vector3d(pos_vec[0], pos_vec[1], pos_vec[2]); + } + } + + // Read rotation offset (quaternion: w, x, y, z) + if (offset_node["rot"]) { + auto rot_vec = offset_node["rot"].as>(); + if (rot_vec.size() == 4) { + // YAML format: (w, x, y, z) + offset.rot = Eigen::Quaterniond(rot_vec[0], rot_vec[1], rot_vec[2], rot_vec[3]); + offset.rot.normalize(); + } + } + } + + return offset; + } + + /** + * @brief Apply offset to ray starts and directions + * @param offset The offset to apply + * + * Applies rotation to directions, then adds position to starts + */ + void apply_offset(const Offset& offset) { + // Apply rotation to directions + for (int i = 0; i < ray_directions_local_.rows(); ++i) { + Vector3d dir = ray_directions_local_.row(i).transpose(); + Vector3d rotated_dir = quat_apply(offset.rot, dir); + ray_directions_local_.row(i) = rotated_dir.transpose(); + } + + // Apply position offset to starts + ray_starts_local_.rowwise() += offset.pos.transpose(); + } + + // Local-frame ray data (set by derived classes in setup_rays) + MatrixX3d ray_starts_local_; // Nx3 matrix of ray start positions + MatrixX3d ray_directions_local_; // Nx3 matrix of ray directions (unit vectors) + + private: + YAML::Node config_; + std::string site_name_; + mjtByte geom_group_mask_[mjNGROUP]; +}; + +} // namespace obelisk diff --git a/obelisk_ws/src/cpp/obelisk_sim_cpp/CMakeLists.txt b/obelisk_ws/src/cpp/obelisk_sim_cpp/CMakeLists.txt index 7cbfd26f..97320b24 100644 --- a/obelisk_ws/src/cpp/obelisk_sim_cpp/CMakeLists.txt +++ b/obelisk_ws/src/cpp/obelisk_sim_cpp/CMakeLists.txt @@ -35,6 +35,28 @@ if(USE_MUJOCO) ament_target_dependencies(obelisk_mujoco_robot PUBLIC rclcpp) + # Demo the raycaster + # Fetch yaml-cpp; disable tests and tools, which are unnecessary and can throw warnings + set(CMAKE_POLICY_DEFAULT_CMP0077 NEW) + set(YAML_CPP_BUILD_TESTS OFF) + set(YAML_CPP_BUILD_TOOLS OFF) + FetchContent_Declare( + yaml-cpp + GIT_REPOSITORY https://github.com/jbeder/yaml-cpp.git + GIT_TAG 0.8.0 # note all other versions are of the form yaml-cpp-x.y.z + ) + FetchContent_MakeAvailable(yaml-cpp) + + # Disable warnings for yaml-cpp + if(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang") + target_compile_options(yaml-cpp PRIVATE -w -Wno-unused-parameter) + elseif(MSVC) + target_compile_options(yaml-cpp PRIVATE /W0 /wd4100) + endif() + add_executable(ray_caster_demo src/ray_caster_demo.cpp) + find_package(Eigen3 REQUIRED) + target_link_libraries(ray_caster_demo PUBLIC Obelisk::Core Eigen3::Eigen yaml-cpp) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) set(ament_cmake_copyright_FOUND TRUE) diff --git a/obelisk_ws/src/cpp/obelisk_sim_cpp/src/ray_caster_demo.cpp b/obelisk_ws/src/cpp/obelisk_sim_cpp/src/ray_caster_demo.cpp new file mode 100644 index 00000000..42c36d24 --- /dev/null +++ b/obelisk_ws/src/cpp/obelisk_sim_cpp/src/ray_caster_demo.cpp @@ -0,0 +1,96 @@ +#include "height_scan_interface.h" +#include "lidar_interface.h" + +#include +#include + +int main(int argc, char* argv[]) { + std::string height_scan_config = "/home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/height_scan_config.yaml"; + std::string lidar_config = "/home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml"; + + // Allow config paths as arguments + if (argc >= 2) { + height_scan_config = argv[1]; + } + if (argc >= 3) { + lidar_config = argv[2]; + } + + std::cout << "=== Height Scan Interface ===" << std::endl; + { + obelisk::HeightScanInterface scanner(height_scan_config); + + std::cout << "Site name: " << scanner.get_site() << std::endl; + std::cout << "Num rays: " << scanner.get_num_rays() << std::endl; + + const auto& starts = scanner.get_ray_starts_local(); + const auto& dirs = scanner.get_ray_directions_local(); + + std::cout << "\nFirst 5 rays (local frame):" << std::endl; + std::cout << std::fixed << std::setprecision(4); + for (int i = 0; i < std::min(5, scanner.get_num_rays()); ++i) { + std::cout << " Ray " << i << ": start=(" + << starts(i, 0) << ", " << starts(i, 1) << ", " << starts(i, 2) + << ") dir=(" + << dirs(i, 0) << ", " << dirs(i, 1) << ", " << dirs(i, 2) + << ")" << std::endl; + } + + // Test world transform with identity + Eigen::Matrix3d identity = Eigen::Matrix3d::Identity(); + Eigen::Vector3d pos(1.0, 2.0, 3.0); + Eigen::Matrix starts_w, dirs_w; + scanner.compute_rays_world(identity, pos, starts_w, dirs_w); + + std::cout << "\nFirst 5 rays (world frame, pos=[1,2,3], identity rotation):" << std::endl; + for (int i = 0; i < std::min(5, scanner.get_num_rays()); ++i) { + std::cout << " Ray " << i << ": start=(" + << starts_w(i, 0) << ", " << starts_w(i, 1) << ", " << starts_w(i, 2) + << ") dir=(" + << dirs_w(i, 0) << ", " << dirs_w(i, 1) << ", " << dirs_w(i, 2) + << ")" << std::endl; + } + } + + + std::cout << "\n=== Lidar Interface ===" << std::endl; + { + obelisk::LidarInterface lidar(lidar_config); + + std::cout << "Site name: " << lidar.get_site() << std::endl; + std::cout << "Num rays: " << lidar.get_num_rays() << std::endl; + + const auto& starts = lidar.get_ray_starts_local(); + const auto& dirs = lidar.get_ray_directions_local(); + + std::cout << "\nFirst 5 rays (local frame):" << std::endl; + std::cout << std::fixed << std::setprecision(4); + for (int i = 0; i < std::min(5, lidar.get_num_rays()); ++i) { + std::cout << " Ray " << i << ": start=(" + << starts(i, 0) << ", " << starts(i, 1) << ", " << starts(i, 2) + << ") dir=(" + << dirs(i, 0) << ", " << dirs(i, 1) << ", " << dirs(i, 2) + << ")" << std::endl; + } + + // Test world transform with 90-degree yaw + Eigen::Matrix3d Rz_90; + Rz_90 << 0, -1, 0, + 1, 0, 0, + 0, 0, 1; + Eigen::Vector3d pos(0.0, 0.0, 1.0); + Eigen::Matrix starts_w, dirs_w; + lidar.compute_rays_world(Rz_90, pos, starts_w, dirs_w); + + std::cout << "\nFirst 5 rays (world frame, pos=[0,0,1], 90deg yaw):" << std::endl; + for (int i = 0; i < std::min(5, lidar.get_num_rays()); ++i) { + std::cout << " Ray " << i << ": start=(" + << starts_w(i, 0) << ", " << starts_w(i, 1) << ", " << starts_w(i, 2) + << ") dir=(" + << dirs_w(i, 0) << ", " << dirs_w(i, 1) << ", " << dirs_w(i, 2) + << ")" << std::endl; + } + } + + return 0; +} diff --git a/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/CMakeLists.txt b/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/CMakeLists.txt index 93e7cda5..ee012ae4 100644 --- a/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/CMakeLists.txt +++ b/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ObkImu.msg" "msg/ObkFramePose.msg" "msg/ObkForceSensor.msg" + "msg/ObkScan.msg" DEPENDENCIES std_msgs geometry_msgs diff --git a/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/msg/ObkScan.msg b/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/msg/ObkScan.msg new file mode 100644 index 00000000..34a3a8c7 --- /dev/null +++ b/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/msg/ObkScan.msg @@ -0,0 +1,6 @@ +# Holds information from a lidar scan or heightmap query + +string MESSAGE_NAME="ObkScan" + +std_msgs/Header header +float32[] data \ No newline at end of file diff --git a/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml new file mode 100644 index 00000000..092f1880 --- /dev/null +++ b/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml @@ -0,0 +1,293 @@ +config: g1_example +onboard: + control: + - pkg: obelisk_unitree_cpp + params: + robot_str: G1 + executable: unitree_example_controller + # callback_groups: + publishers: + - ros_parameter: pub_ctrl_setting + topic: /obelisk/g1/ctrl + history_depth: 10 + callback_group: None + subscribers: + - ros_parameter: sub_est_setting + topic: /obelisk/g1/est_state + history_depth: 10 + callback_group: None + # ----- Joystick subscriber ----- # + - ros_parameter: joystick_sub_setting + topic: /obelisk/g1/joy + timers: + - ros_parameter: timer_ctrl_setting + timer_period_sec: 0.001 + callback_group: None + # ----- High Level/Execution FSM Controller ----- # + - pkg: obelisk_unitree_cpp + executable: obelisk_unitree_joystick + # callback_groups: + publishers: + # ----- Execution FSM ----- # + - ros_parameter: pub_exec_fsm_setting + topic: /obelisk/g1/exec_fsm + history_depth: 10 + callback_group: None + # ----- High Level Control ----- # + - ros_parameter: pub_ctrl_setting + topic: /obelisk/g1/vel_cmd + history_depth: 10 + callback_group: None + # ----- Joystick Passthrough ----- # + - ros_parameter: pub_joy_passthrough_setting + topic: /obelisk/g1/joy_passthrough + history_depth: 10 + callback_group: None + subscribers: + # ----- Joystick subscriber ----- # + - ros_parameter: sub_est_setting + topic: /obelisk/g1/joy + timers: + - ros_parameter: timer_ctrl_setting + timer_period_sec: 0.1 + callback_group: None + estimation: + - pkg: obelisk_unitree_cpp + executable: unitree_example_estimator + # callback_groups: + publishers: + - ros_parameter: pub_est_setting + topic: /obelisk/g1/est_state + msg_type: EstimatedState + history_depth: 10 + callback_group: None + subscribers: + - ros_parameter: sub_sensor_setting + topic: /obelisk/g1/joint_encoders + msg_type: ObkJointEncoders + history_depth: 10 + callback_group: None + timers: + - ros_parameter: timer_est_setting + timer_period_sec: 0.001 + callback_group: None + # sensing: + robot: + # === simulation === + - is_simulated: True + pkg: obelisk_unitree_cpp + executable: obelisk_unitree_sim + params: + ic_keyframe: standing + # === hardware === + # - is_simulated: False + # pkg: obelisk_unitree_cpp + # executable: obelisk_unitree_g1_hardware + # params: + # # network_interface_name: enx6c1ff724e92e + # default_joint_names: [ + # "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint", + # "left_knee_joint", "left_ankle_pitch_joint", "left_ankle_roll_joint", + # "right_hip_pitch_joint", "right_hip_roll_joint", "right_hip_yaw_joint", + # "right_knee_joint", "right_ankle_pitch_joint", "right_ankle_roll_joint", + # "waist_yaw_joint", + # "left_shoulder_pitch_joint", "left_shoulder_roll_joint", "left_shoulder_yaw_joint", "left_elbow_joint", + # "left_wrist_roll_joint", "left_wrist_pitch_joint", "left_wrist_yaw_joint", + # "right_shoulder_pitch_joint", "right_shoulder_roll_joint", "right_shoulder_yaw_joint", "right_elbow_joint", + # "right_wrist_roll_joint", "right_wrist_pitch_joint", "right_wrist_yaw_joint", + # ] + # user_pose: [ + # -0.25, 0, 0, + # 0.46, -0.25, 0, + # -0.25, 0, 0, + # 0.46, -0.25, 0, + # 0, + # 0.07, 0.24, 0, 1.39, + # 0, 0, 0, + # 0.07, -0.24, 0, 1.39, + # 0, 0, 0 + # ] + # default_kp: [ + # 100., 100., 100., # L hip + # 150., 100., 100., # L knee, ankle + # 100., 100., 100., # R hip + # 150., 100., 100., # R knee, ankle + # 100., # Waist + # 50., 50., 40., 40., # L shoulder, elbow + # 30., 30., 30., # L wrist + # 50., 50., 40., 40., # R shoulder, elbow + # 30., 30., 30., # R wrist + # ] + # default_kd: [ + # 2., 2., 2., # L hip + # 4., 2., 2., # L knee, ankle + # 2., 2., 2., # R hip + # 4., 2., 2., # R knee, ankle + # 2., # Waist + # 2., 2., 2., 2., # L shoulder, elbow + # 2., 2., 2., # L wrist + # 2., 2., 2., 2., # R shoulder, elbow + # 2., 2., 2., # R wrist + # ] + # default_kd_damping: [ + # 10., 10., 10., # L hip + # 10., 10., 10., # L knee, ankle + # 10., 10., 10., # R hip + # 10., 10., 10., # R knee, ankle + # 10., # Waist + # 10., 10., 10., 10., # L shoulder, elbow + # 10., 10., 10., # L wrist + # 10., 10., 10., 10., # R shoulder, elbow + # 10., 10., 10., # R wrist + # ] + # ================== + # callback_groups: + publishers: + # ----- Joints ----- # + - ros_parameter: pub_sensor_setting + topic: /obelisk/g1/joint_encoders + history_depth: 10 + # ----- IMU ----- # + - ros_parameter: pub_imu_setting + topic: /obelisk/g1/pelvis_imu + history_depth: 10 + # ----- Odom ----- # + - ros_parameter: pub_odom_setting + topic: /obelisk/g1/odom + history_depth: 10 + subscribers: + # ----- Control ----- # + - ros_parameter: sub_ctrl_setting + topic: /obelisk/g1/ctrl + history_depth: 10 + callback_group: None + # ----- Execution FSM ----- # + - ros_parameter: sub_fsm_setting + topic: /obelisk/g1/exec_fsm + history_depth: 10 + callback_group: None + # ----- High Level Control ----- # + - ros_parameter: sub_vel_cmd_setting + topic: /obelisk/g1/vel_cmd + history_depth: 10 + callback_group: None + sim: + - ros_parameter: mujoco_setting + n_u: 16 + time_step: 0.002 + num_steps_per_viz: 5 + robot_pkg: g1_description + # model_xml_path: g1_27dof_fixed_base.xml + # model_xml_path: g1_27dof.xml + model_xml_path: g1_basic_scene.xml + sensor_settings: + - topic: /obelisk/g1/raycast + dt: 0.1 + msg_type: ObkScan + config_path: /home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/height_scan_config.yaml + sensor_names: + scan_pos_sensor: framepos + - topic: /obelisk/g1/joint_encoders + dt: 0.002 + msg_type: ObkJointEncoders + sensor_names: + # ---------- Joint Positions ---------- # + left_hip_pitch_joint_pos_sensor: jointpos + left_hip_roll_joint_pos_sensor: jointpos + left_hip_yaw_joint_pos_sensor: jointpos + left_knee_joint_pos_sensor: jointpos + left_ankle_pitch_joint_pos_sensor: jointpos + left_ankle_roll_joint_pos_sensor: jointpos + + right_hip_pitch_joint_pos_sensor: jointpos + right_hip_roll_joint_pos_sensor: jointpos + right_hip_yaw_joint_pos_sensor: jointpos + right_knee_joint_pos_sensor: jointpos + right_ankle_pitch_joint_pos_sensor: jointpos + right_ankle_roll_joint_pos_sensor: jointpos + + waist_yaw_joint_pos_sensor: jointpos + + left_shoulder_pitch_joint_pos_sensor: jointpos + left_shoulder_roll_joint_pos_sensor: jointpos + left_shoulder_yaw_joint_pos_sensor: jointpos + left_elbow_joint_pos_sensor: jointpos + left_wrist_roll_joint_pos_sensor: jointpos + left_wrist_pitch_joint_pos_sensor: jointpos + left_wrist_yaw_joint_pos_sensor: jointpos + + right_shoulder_pitch_joint_pos_sensor: jointpos + right_shoulder_roll_joint_pos_sensor: jointpos + right_shoulder_yaw_joint_pos_sensor: jointpos + right_elbow_joint_pos_sensor: jointpos + right_wrist_roll_joint_pos_sensor: jointpos + right_wrist_pitch_joint_pos_sensor: jointpos + right_wrist_yaw_joint_pos_sensor: jointpos + + # ---------- Joint Velocities ---------- # + left_hip_pitch_joint_vel_sensor: jointvel + left_hip_roll_joint_vel_sensor: jointvel + left_hip_yaw_joint_vel_sensor: jointvel + left_knee_joint_vel_sensor: jointvel + left_ankle_pitch_joint_vel_sensor: jointvel + left_ankle_roll_joint_vel_sensor: jointvel + + right_hip_pitch_joint_vel_sensor: jointvel + right_hip_roll_joint_vel_sensor: jointvel + right_hip_yaw_joint_vel_sensor: jointvel + right_knee_joint_vel_sensor: jointvel + right_ankle_pitch_joint_vel_sensor: jointvel + right_ankle_roll_joint_vel_sensor: jointvel + + waist_yaw_joint_vel_sensor: jointvel + + left_shoulder_pitch_joint_vel_sensor: jointvel + left_shoulder_roll_joint_vel_sensor: jointvel + left_shoulder_yaw_joint_vel_sensor: jointvel + left_elbow_joint_vel_sensor: jointvel + left_wrist_roll_joint_vel_sensor: jointvel + left_wrist_pitch_joint_vel_sensor: jointvel + left_wrist_yaw_joint_vel_sensor: jointvel + + right_shoulder_pitch_joint_vel_sensor: jointvel + right_shoulder_roll_joint_vel_sensor: jointvel + right_shoulder_yaw_joint_vel_sensor: jointvel + right_elbow_joint_vel_sensor: jointvel + right_wrist_roll_joint_vel_sensor: jointvel + right_wrist_pitch_joint_vel_sensor: jointvel + right_wrist_yaw_joint_vel_sensor: jointvel + timers: + - ros_parameter: timer_sensor_setting + timer_period_sec: 0.02 + callback_group: None + - ros_parameter: timer_logging_setting + timer_period_sec: 0.2 + callback_group: None + viz: + on: True + viz_tool: foxglove + viz_nodes: + - pkg: obelisk_viz_cpp + executable: default_robot_viz + robot_pkg: g1_description + urdf: g1.urdf + robot_topic: robot_description + subscribers: + - ros_parameter: sub_viz_est_setting + topic: /obelisk/g1/est_state + history_depth: 10 + callback_group: None + non_obelisk: False + publishers: + - ros_parameter: pub_viz_joint_setting + topic: joint_states + history_depth: 10 + callback_group: None + timers: + - ros_parameter: timer_viz_joint_setting + timer_period_sec: 0.01 + callback_group: None + joystick: + on: True + pub_topic: /obelisk/g1/joy + sub_topic: /obelisk/g1/joy_feedback \ No newline at end of file diff --git a/obelisk_ws/src/obelisk_ros/config/height_scan_config.yaml b/obelisk_ws/src/obelisk_ros/config/height_scan_config.yaml new file mode 100644 index 00000000..728faed4 --- /dev/null +++ b/obelisk_ws/src/obelisk_ros/config/height_scan_config.yaml @@ -0,0 +1,10 @@ +site_name: "waist_yaw_link" +geom_groups: [0] + +pattern: + type: "height_scan" + ordering: "xy" + resolution: 0.1 + size: [1.6, 0.8] + offset: + pos: [0.08, 0.0, 20.0] diff --git a/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml b/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml new file mode 100644 index 00000000..4ea4fbe9 --- /dev/null +++ b/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml @@ -0,0 +1,12 @@ +site_name: "lidar_site" +geom_groups: [0, 1] + +pattern: + pattern_name: "lidar" + vertical_fov_range: [-25.0, 25.0] + horizontal_fov_range: [-25.0, 25.0] + channels: 21 + horizontal_res: 2.5 + offset: + pos: [0.08, 0.0, 0.15] + rot: [0.8660, 0.0, 0.5000, 0.0] diff --git a/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof.xml b/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof.xml index 5f85bc14..fccfaa14 100644 --- a/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof.xml +++ b/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof.xml @@ -799,6 +799,9 @@ Position and velocity actuators added. --> + + + From 75042de05720f825a94e3c992240c2e9da48f59f Mon Sep 17 00:00:00 2001 From: Will Compton Date: Thu, 29 Jan 2026 15:09:38 -0800 Subject: [PATCH 02/20] functional heightscan --- .../include/obelisk_mujoco_sim_robot.h | 36 ++++++++++--------- .../src/obelisk_ros/config/g1_scan_cpp.yaml | 8 ++++- .../config/height_scan_config.yaml | 4 +-- .../src/obelisk_ros/config/lidar_config.yaml | 6 ++-- .../g1_description/mujoco/g1_basic_scene.xml | 2 +- 5 files changed, 33 insertions(+), 23 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 index 8c6b413a..96082e4d 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h @@ -299,6 +299,19 @@ namespace obelisk { std::scoped_lock lock(sensor_data_mut_, shared_data_mut_); time = data_->time; mjv_updateScene(model_, data_, &opt, NULL, &cam, mjCAT_ALL, &scn); + + mjtNum size[3] = {0.01, 0, 0}; + mjtNum mat[9] = {1,0,0, 0,1,0, 0,0,1}; + float rgba[4] = {1,0,0,1}; + + for (auto &p : scan_viz_points_) { + if (scn.ngeom >= scn.maxgeom) break; + + mjvGeom g; + mjv_initGeom(&g, mjGEOM_SPHERE, size, p.data(), mat, rgba); + scn.geoms[scn.ngeom++] = g; + } + } mjr_render(viewport, &scn, &con); @@ -480,6 +493,7 @@ namespace obelisk { sensor_config_path = it->second; } + std::vector sensor_names; std::vector mj_sensor_types; const std::string name_delim = "&"; @@ -582,20 +596,10 @@ namespace obelisk { } else { RCLCPP_ERROR_STREAM( this->get_logger(), - "Provided RayCaster Scan type " + type_str + "is invalid. Valid types are 'height_scan' and 'lidar_scan'" + "Provided RayCaster Scan type " + type_str + " is invalid. Valid types are 'height_scan' and 'lidar_scan'" ); } - scan_dots_idx_ = scn.ngeom; - scn.ngeom += scan_interface_->get_num_rays(); - mjtNum size[3] = {0.01, 0, 0}; // sphere uses size[0] - float rgba[4] = {1, 0, 0, 1}; // red - for (int ii = 0; ii < scan_interface_->get_num_rays(); ++ii) { - mjvGeom g; - mjv_initGeom(&g, mjGEOM_SPHERE, size, nullptr, nullptr, rgba); - scn.geoms[scan_dots_idx_ + ii] = g; - } - // Add the timer to the list this->timers_[sensor_key] = this->create_wall_timer( std::chrono::milliseconds(static_cast(1e3 * dt)), @@ -1195,6 +1199,9 @@ namespace obelisk { scan_interface_->compute_rays_world(rot, pos, starts_w, dirs_w); + scan_viz_points_.clear(); + scan_viz_points_.reserve(scan_interface_->get_num_rays()); + for (int ii = 0; ii < scan_interface_->get_num_rays(); ++ii) { Eigen::Vector3d ray_origin = starts_w.row(ii).transpose(); Eigen::Vector3d direction = dirs_w.row(ii).transpose(); @@ -1210,11 +1217,7 @@ namespace obelisk { }; float ret = scan_interface_->get_return(hit_point, dist); msg.data.push_back(ret); - mjvGeom& g = scn.geoms[scan_dots_idx_ + ii]; - g.type = mjGEOM_SPHERE; - g.pos[0] = hit_point[0]; g.pos[1] = hit_point[1]; g.pos[2] = hit_point[2]; - g.size[0] = 0.01; - g.rgba[0] = 1.f; g.rgba[1] = 0.f; g.rgba[2] = 0.f; g.rgba[3] = 1.f; + scan_viz_points_.push_back({hit_point[0], hit_point[1], hit_point[2]}); } msg.header.frame_id = "world"; @@ -1372,6 +1375,7 @@ namespace obelisk { // Geom id's for viz std::vector viz_geoms_; + std::vector> scan_viz_points_; // Shared data between the main thread and the sim thread std::vector shared_data_; diff --git a/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml index 092f1880..979afe36 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml @@ -181,10 +181,16 @@ onboard: # model_xml_path: g1_27dof.xml model_xml_path: g1_basic_scene.xml sensor_settings: + # - topic: /obelisk/g1/raycast + # dt: 0.1 + # msg_type: ObkScan + # config_path: /home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/height_scan_config.yaml + # sensor_names: + # scan_pos_sensor: framepos - topic: /obelisk/g1/raycast dt: 0.1 msg_type: ObkScan - config_path: /home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/height_scan_config.yaml + config_path: /home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml sensor_names: scan_pos_sensor: framepos - topic: /obelisk/g1/joint_encoders diff --git a/obelisk_ws/src/obelisk_ros/config/height_scan_config.yaml b/obelisk_ws/src/obelisk_ros/config/height_scan_config.yaml index 728faed4..d9cae7b8 100644 --- a/obelisk_ws/src/obelisk_ros/config/height_scan_config.yaml +++ b/obelisk_ws/src/obelisk_ros/config/height_scan_config.yaml @@ -1,5 +1,5 @@ -site_name: "waist_yaw_link" -geom_groups: [0] +site_name: "pelvis_mocap_site" +geom_groups: [2] pattern: type: "height_scan" diff --git a/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml b/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml index 4ea4fbe9..af74f7b6 100644 --- a/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml +++ b/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml @@ -1,8 +1,8 @@ -site_name: "lidar_site" -geom_groups: [0, 1] +site_name: "pelvis_mocap_site" +geom_groups: [0, 1, 2] pattern: - pattern_name: "lidar" + type: "lidar_scan" vertical_fov_range: [-25.0, 25.0] horizontal_fov_range: [-25.0, 25.0] channels: 21 diff --git a/obelisk_ws/src/robots/g1_description/mujoco/g1_basic_scene.xml b/obelisk_ws/src/robots/g1_description/mujoco/g1_basic_scene.xml index 3123f071..93805e4e 100644 --- a/obelisk_ws/src/robots/g1_description/mujoco/g1_basic_scene.xml +++ b/obelisk_ws/src/robots/g1_description/mujoco/g1_basic_scene.xml @@ -17,6 +17,6 @@ - + \ No newline at end of file From 1bff164453b0b69261a17ccc372d48508c61c619 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Thu, 5 Feb 2026 14:24:24 -0800 Subject: [PATCH 03/20] off by one error corrected in lidar interface --- obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h b/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h index a220ae04..2e0a8c63 100644 --- a/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h +++ b/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h @@ -108,7 +108,7 @@ class LidarInterface : public RayCasterInterface { // Compute horizontal angles double horz_span = horz_max_deg - horz_min_deg; int num_horizontal = - static_cast(std::ceil(horz_span / horz_res_deg)) + 1; // +1 for endpoints + static_cast(std::ceil(horz_span / horz_res_deg)); // Isaac doesn't add +1 for endpoints... // Handle 360-degree wraparound: exclude last point to avoid overlap bool is_full_rotation = std::abs(std::abs(horz_span) - 360.0) < 1e-6; From 78cd058fbd1cb67acf439dbcd6ccb06ef30e5a77 Mon Sep 17 00:00:00 2001 From: Zach Olkin Date: Sun, 15 Feb 2026 12:31:38 -0800 Subject: [PATCH 04/20] [WIP] fix for the virtual method call in the unitree logging --- .../cpp/hardware/robots/unitree/unitree_interface.h | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h index 100c1803..79504b3f 100644 --- a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h @@ -88,10 +88,10 @@ namespace obelisk { auto time_t = std::chrono::system_clock::to_time_t(now); std::stringstream ss; ss << std::put_time(std::localtime(&time_t), "%Y-%m-%d_%H-%M-%S"); - + std::filesystem::path unitree_logs_dir = "unitree_" + robot_name + "_logs"; std::filesystem::path session_dir = unitree_logs_dir / ss.str(); - + std::filesystem::create_directories(session_dir); log_dir_path_ = session_dir.string(); RCLCPP_INFO_STREAM(this->get_logger(), "Created logging directory: " << session_dir); @@ -99,8 +99,8 @@ namespace obelisk { log_count_ = 0; startup_time_ = this->now(); - InitializeLogging(); - // TODO: Tie timer callback to WriteMotorData() + // NOTE: InitializeLogging and timer registration are deferred to on_activate + // because they call pure virtual methods that aren't available during construction. this->RegisterObkTimer("timer_logging_setting", timer_logging_key_, std::bind(&ObeliskUnitreeInterface::WriteMotorData, this)); } @@ -117,6 +117,10 @@ namespace obelisk { ReleaseUnitreeMotionControl(); } + if (logging_) { + InitializeLogging(); + } + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } From 0e5fbd03c706f497a2e4b7309d95e0e237d77175 Mon Sep 17 00:00:00 2001 From: Zach Olkin Date: Sun, 15 Feb 2026 12:36:26 -0800 Subject: [PATCH 05/20] Updated the mujoco sim to publish the normal IMU mesaage --- .../include/obelisk_mujoco_sim_robot.h | 15 ++++++++------- obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml | 2 +- .../src/obelisk_ros/config/dummy_cpp_viz.yaml | 2 +- .../src/obelisk_ros/config/dummy_cpp_zed.yaml | 2 +- 4 files changed, 11 insertions(+), 10 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 index 0c483163..23071849 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h @@ -16,6 +16,7 @@ #include #include #include +#include #include #include "ament_index_cpp/get_package_share_directory.hpp" @@ -519,18 +520,18 @@ namespace obelisk { sensor_names, mj_sensor_types, this->template GetPublisher(sensor_key)), callback_group_); - } else if (sensor_type == obelisk_sensor_msgs::msg::ObkImu::MESSAGE_NAME) { + } else if (sensor_type == "Imu") { // Make a publisher and add it to the list - auto pub = ObeliskNode::create_publisher(topic, depth); + auto pub = ObeliskNode::create_publisher(topic, depth); this->publishers_[sensor_key] = - std::make_shared>(pub); + std::make_shared>(pub); // Add the timer to the list this->timers_[sensor_key] = this->create_wall_timer( std::chrono::milliseconds(static_cast(1e3 * dt)), - CreateTimerCallback( + CreateTimerCallback( sensor_names, mj_sensor_types, - this->template GetPublisher(sensor_key)), + this->template GetPublisher(sensor_key)), callback_group_); } else if (sensor_type == "PoseStamped") { // Make a publisher and add it to the list @@ -827,12 +828,12 @@ namespace obelisk { RCLCPP_INFO_STREAM(this->get_logger(), "Timer callback created for JointEncoders!"); return cb; - } else if constexpr (std::is_same::value) { + } else if constexpr (std::is_same::value) { // ----------------------------------- // // ---------- IMU call back ---------- // // ----------------------------------- // auto cb = [publisher, sensor_names, mj_sensor_types, this]() { - obelisk_sensor_msgs::msg::ObkImu msg; + sensor_msgs::msg::Imu msg; // TODO: For now all the covariances are 0 // *** Note *** Mujoco does not support adding noise natively since v3.14. diff --git a/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml index a80a3bb3..96e83374 100644 --- a/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/dummy_cpp.yaml @@ -79,7 +79,7 @@ onboard: joint_vel: jointvel - topic: /obelisk/dummy/imu dt: 0.002 - msg_type: ObkImu + msg_type: Imu sensor_names: tip_acc_sensor: accelerometer tip_gyro_sensor: gyro diff --git a/obelisk_ws/src/obelisk_ros/config/dummy_cpp_viz.yaml b/obelisk_ws/src/obelisk_ros/config/dummy_cpp_viz.yaml index 68def487..9694ab2e 100644 --- a/obelisk_ws/src/obelisk_ros/config/dummy_cpp_viz.yaml +++ b/obelisk_ws/src/obelisk_ros/config/dummy_cpp_viz.yaml @@ -76,7 +76,7 @@ onboard: joint_vel: jointvel - topic: /obelisk/dummy/imu dt: 0.002 - msg_type: ObkImu + msg_type: Imu sensor_names: tip_acc_sensor: accelerometer tip_gyro_sensor: gyro diff --git a/obelisk_ws/src/obelisk_ros/config/dummy_cpp_zed.yaml b/obelisk_ws/src/obelisk_ros/config/dummy_cpp_zed.yaml index bae4eafd..6a5ff125 100644 --- a/obelisk_ws/src/obelisk_ros/config/dummy_cpp_zed.yaml +++ b/obelisk_ws/src/obelisk_ros/config/dummy_cpp_zed.yaml @@ -93,7 +93,7 @@ onboard: joint_vel: jointvel - topic: /obelisk/dummy/imu dt: 0.002 - msg_type: ObkImu + msg_type: Imu sensor_names: tip_acc_sensor: accelerometer tip_gyro_sensor: gyro From ee13ea25112cd312ff16746cdafc4e50c5b3125a Mon Sep 17 00:00:00 2001 From: Zach Olkin Date: Sun, 15 Feb 2026 13:11:29 -0800 Subject: [PATCH 06/20] updated the error message --- obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h index 23071849..a0ba47b3 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h @@ -573,7 +573,7 @@ namespace obelisk { this->template GetPublisher(sensor_key)), callback_group_); } else { - throw std::runtime_error("Sensor type not supported!"); + throw std::runtime_error("Sensor type not supported: " + sensor_type); } this->timers_[sensor_key]->cancel(); // Stop the timer From e054e336c962d5781fa090d8d84b1634bd05b578 Mon Sep 17 00:00:00 2001 From: Zach Olkin Date: Sun, 15 Feb 2026 15:37:31 -0800 Subject: [PATCH 07/20] removed the obk imus from the unitree interface --- obelisk/cpp/hardware/robots/unitree/g1_interface.h | 4 ++-- obelisk/cpp/hardware/robots/unitree/go2_interface.h | 4 ++-- obelisk/cpp/hardware/robots/unitree/unitree_interface.h | 3 ++- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/obelisk/cpp/hardware/robots/unitree/g1_interface.h b/obelisk/cpp/hardware/robots/unitree/g1_interface.h index 62f34ad5..0b17510c 100644 --- a/obelisk/cpp/hardware/robots/unitree/g1_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/g1_interface.h @@ -253,7 +253,7 @@ namespace obelisk { this->GetPublisher(pub_joint_state_key_)->publish(joint_state); // IMU - obelisk_sensor_msgs::msg::ObkImu imu_state; + sensor_msgs::msg::Imu imu_state; imu_state.header.stamp = this->now(); imu_state.angular_velocity.x = low_state.imu_state().gyroscope()[0]; imu_state.angular_velocity.y = low_state.imu_state().gyroscope()[1]; @@ -268,7 +268,7 @@ namespace obelisk { imu_state.linear_acceleration.y= low_state.imu_state().accelerometer()[1]; imu_state.linear_acceleration.z = low_state.imu_state().accelerometer()[2]; - this->GetPublisher(pub_imu_state_key_)->publish(imu_state); + this->GetPublisher(pub_imu_state_key_)->publish(imu_state); // Log motor data if logging is enabled if (logging_) { diff --git a/obelisk/cpp/hardware/robots/unitree/go2_interface.h b/obelisk/cpp/hardware/robots/unitree/go2_interface.h index 40b6a515..fa5a7a16 100644 --- a/obelisk/cpp/hardware/robots/unitree/go2_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/go2_interface.h @@ -195,7 +195,7 @@ namespace obelisk { this->GetPublisher(pub_joint_state_key_)->publish(joint_state); // IMU - obelisk_sensor_msgs::msg::ObkImu imu_state; + sensor_msgs::msg::Imu imu_state; imu_state.header.stamp = this->now(); imu_state.angular_velocity.x = low_state.imu_state().gyroscope()[0]; imu_state.angular_velocity.y = low_state.imu_state().gyroscope()[1]; @@ -210,7 +210,7 @@ namespace obelisk { imu_state.linear_acceleration.y= low_state.imu_state().accelerometer()[1]; imu_state.linear_acceleration.z = low_state.imu_state().accelerometer()[2]; - this->GetPublisher(pub_imu_state_key_)->publish(imu_state); + this->GetPublisher(pub_imu_state_key_)->publish(imu_state); if (logging_) { std::lock_guard lk(motor_state_mtx_); diff --git a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h index 79504b3f..8edf1dda 100644 --- a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h @@ -7,6 +7,7 @@ #include "obelisk_control_msgs/msg/pd_feed_forward.h" #include "obelisk_control_msgs/msg/execution_fsm.hpp" #include "obelisk_control_msgs/msg/velocity_command.hpp" +#include #include #include #include @@ -54,7 +55,7 @@ namespace obelisk { // Additional Publishers this->RegisterObkPublisher("pub_sensor_setting", pub_joint_state_key_); - this->RegisterObkPublisher("pub_imu_setting", pub_imu_state_key_); + this->RegisterObkPublisher("pub_imu_setting", pub_imu_state_key_); // Register Execution FSM Subscriber this->RegisterObkSubscription( From d9830c59cc82e562d000e634efaaa1cc4f7f4310 Mon Sep 17 00:00:00 2001 From: Zach Olkin Date: Mon, 16 Feb 2026 09:11:18 -0800 Subject: [PATCH 08/20] Added motor temps to the joint encoder message type and made an option to publish it with the unitree interface --- obelisk/cpp/hardware/robots/unitree/g1_interface.h | 11 +++++++++++ .../cpp/hardware/robots/unitree/unitree_interface.h | 7 +++++++ .../obelisk_sensor_msgs/msg/ObkJointEncoders.msg | 2 ++ 3 files changed, 20 insertions(+) diff --git a/obelisk/cpp/hardware/robots/unitree/g1_interface.h b/obelisk/cpp/hardware/robots/unitree/g1_interface.h index 0b17510c..65aed5ce 100644 --- a/obelisk/cpp/hardware/robots/unitree/g1_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/g1_interface.h @@ -230,6 +230,11 @@ namespace obelisk { joint_state.joint_vel.resize(num_motors_); joint_state.joint_names.resize(num_motors_); + if (this->pub_temps_) { + joint_state.motor_surface_temps.resize(num_motors_); + joint_state.motor_winding_temps.resize(num_motors_); + } + size_t ind = 0; for (size_t i = 0; i < G1_27DOF + G1_EXTRA_WAIST; ++i) { { @@ -246,6 +251,12 @@ namespace obelisk { joint_state.joint_names.at(ind) = G1_29DOF_JOINT_NAMES[i]; joint_state.joint_pos.at(ind) = low_state.motor_state()[i].q(); joint_state.joint_vel.at(ind) = low_state.motor_state()[i].dq(); + + if (this->pub_temps_) { + joint_state.motor_surface_temps.at(ind) = low_state.motor_state()[i].temperature()[0]; + joint_state.motor_winding_temps.at(ind) = low_state.motor_state()[i].temperature()[1]; + + } ind++; } diff --git a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h index 8edf1dda..a1dd7cf2 100644 --- a/obelisk/cpp/hardware/robots/unitree/unitree_interface.h +++ b/obelisk/cpp/hardware/robots/unitree/unitree_interface.h @@ -105,6 +105,10 @@ namespace obelisk { this->RegisterObkTimer("timer_logging_setting", timer_logging_key_, std::bind(&ObeliskUnitreeInterface::WriteMotorData, this)); } + + // Optional motor temperature publisher + this->declare_parameter("pub_motor_temp", false); + pub_temps_ = this->get_parameter("pub_motor_temp").as_bool(); } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn virtual on_activate( @@ -306,6 +310,9 @@ namespace obelisk { std::array motor_state_; mutable std::mutex motor_state_mtx_; + // Temp publishing + bool pub_temps_; + // ---------- Gains ---------- // std::vector kp_; std::vector kd_; diff --git a/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/msg/ObkJointEncoders.msg b/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/msg/ObkJointEncoders.msg index 0b72949c..bb4c91c4 100644 --- a/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/msg/ObkJointEncoders.msg +++ b/obelisk_ws/src/obelisk_msgs/obelisk_sensor_msgs/msg/ObkJointEncoders.msg @@ -8,4 +8,6 @@ builtin_interfaces/Time stamp # TODO: remove float64[] joint_pos float64[] joint_vel +float64[] motor_surface_temps +float64[] motor_winding_temps string[] joint_names From f79bca6a5455a7cacb5d1647b625fbc9fdc4b67d Mon Sep 17 00:00:00 2001 From: Will Compton Date: Tue, 17 Feb 2026 14:04:47 -0800 Subject: [PATCH 09/20] depth scan and standard imu --- .../include/obelisk_mujoco_sim_robot.h | 209 ++++++++++++++++++ .../cpp/obelisk_cpp/lidar/lidar_interface.h | 10 + .../obelisk_cpp/lidar/ray_caster_interface.h | 3 + .../src/obelisk_ros/config/depth_config.yaml | 12 + .../src/obelisk_ros/config/g1_scan_cpp.yaml | 26 ++- .../robots/g1_description/mujoco/g1_27dof.xml | 13 +- 6 files changed, 264 insertions(+), 9 deletions(-) create mode 100644 obelisk_ws/src/obelisk_ros/config/depth_config.yaml diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h index 96082e4d..f200d803 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h @@ -7,7 +7,9 @@ #include #include +#include #include +#include #include #include @@ -17,6 +19,8 @@ #include #include #include +#include +#include #include "ament_index_cpp/get_package_share_directory.hpp" @@ -548,6 +552,19 @@ namespace obelisk { sensor_names, mj_sensor_types, this->template GetPublisher(sensor_key)), callback_group_); + } else if (sensor_type == "Imu") { + // Make a publisher and add it to the list + auto pub = ObeliskNode::create_publisher(topic, depth); + this->publishers_[sensor_key] = + std::make_shared>(pub); + + // Add the timer to the list + this->timers_[sensor_key] = this->create_wall_timer( + std::chrono::milliseconds(static_cast(1e3 * dt)), + CreateTimerCallback( + sensor_names, mj_sensor_types, + this->template GetPublisher(sensor_key)), + callback_group_); } else if (sensor_type == "PoseStamped") { // Make a publisher and add it to the list auto pub = ObeliskNode::create_publisher(topic, depth); @@ -607,6 +624,40 @@ namespace obelisk { sensor_names, mj_sensor_types, this->template GetPublisher(sensor_key)), callback_group_); + } else if (sensor_type == "DepthImage") { + // Make a publisher and add it to the list + auto pub = ObeliskNode::create_publisher(topic, depth); + this->publishers_[sensor_key] = + std::make_shared>(pub); + + // Create the scanner interface + YAML::Node scan_config = YAML::LoadFile(sensor_config_path); + const auto type_node = scan_config["pattern"]["type"]; + if (!type_node) { + throw std::runtime_error("scan config missing pattern.type"); + } + + const std::string type_str = type_node.as(); + if (type_str == "lidar_scan") { + depth_scan_interface_ = std::make_unique(sensor_config_path); + } else { + RCLCPP_ERROR_STREAM( + this->get_logger(), + "DepthImage sensor type only supports 'lidar_scan' pattern, got: " + type_str + ); + } + + if (depth_scan_interface_->get_image_width() < 0 || depth_scan_interface_->get_image_height() < 0) { + throw std::runtime_error("DepthImage sensor requires a scan interface with image dimensions"); + } + + // Add the timer to the list + this->timers_[sensor_key] = this->create_wall_timer( + std::chrono::milliseconds(static_cast(1e3 * dt)), + CreateTimerCallback( + sensor_names, mj_sensor_types, + this->template GetPublisher(sensor_key)), + callback_group_); } else { throw std::runtime_error("Sensor type not supported!"); } @@ -944,6 +995,79 @@ namespace obelisk { RCLCPP_INFO_STREAM(this->get_logger(), "Timer callback created for a IMU!"); return cb; + } else if constexpr (std::is_same::value) { + // ------------------------------------------ // + // -------- Standard IMU call back ---------- // + // ------------------------------------------ // + auto cb = [publisher, sensor_names, mj_sensor_types, this]() { + sensor_msgs::msg::Imu msg; + + std::lock_guard lock(sensor_data_mut_); + bool has_acc = false; + bool has_gyro = false; + bool has_framequat = false; + + for (size_t i = 0; i < sensor_names.size(); i++) { + int sensor_id = mj_name2id(this->model_, mjOBJ_SENSOR, sensor_names.at(i).c_str()); + if (sensor_id == -1) { + throw std::runtime_error("Sensor not found in Mujoco! Make sure your XML has the sensor."); + } + + int sensor_addr = this->model_->sensor_adr[sensor_id]; + if (mj_sensor_types.at(i) == "accelerometer") { + if (!has_acc) { + msg.linear_acceleration.x = this->data_->sensordata[sensor_addr]; + msg.linear_acceleration.y = this->data_->sensordata[sensor_addr + 1]; + msg.linear_acceleration.z = this->data_->sensordata[sensor_addr + 2]; + + int site_id = this->model_->sensor_objid[sensor_id]; + msg.header.frame_id = + std::string(this->model_->names + this->model_->name_siteadr[site_id]); + + has_acc = true; + } else { + RCLCPP_ERROR_STREAM(this->get_logger(), + "There are two accelerometers associated with this IMU! Ignoring " + << sensor_names.at(i)); + } + } else if (mj_sensor_types.at(i) == "gyro") { + if (!has_gyro) { + msg.angular_velocity.x = this->data_->sensordata[sensor_addr]; + msg.angular_velocity.y = this->data_->sensordata[sensor_addr + 1]; + msg.angular_velocity.z = this->data_->sensordata[sensor_addr + 2]; + has_gyro = true; + } else { + RCLCPP_ERROR_STREAM(this->get_logger(), + "There are two gyros associated with this IMU! Ignoring " + << sensor_names.at(i)); + } + } else if (mj_sensor_types.at(i) == "framequat") { + if (!has_framequat) { + msg.orientation.w = this->data_->sensordata[sensor_addr]; + msg.orientation.x = this->data_->sensordata[sensor_addr + 1]; + msg.orientation.y = this->data_->sensordata[sensor_addr + 2]; + msg.orientation.z = this->data_->sensordata[sensor_addr + 3]; + has_framequat = true; + } else { + RCLCPP_ERROR_STREAM(this->get_logger(), + "There are two framequats associated with this IMU! Ignoring " + << sensor_names.at(i)); + } + } else { + RCLCPP_ERROR_STREAM( + this->get_logger(), + "Sensor " << sensor_names.at(i) + << " is not associated with a valid Mujoco sensor type! Current sensor type: " + << mj_sensor_types.at(i)); + } + + msg.header.stamp = this->now(); + } + publisher->publish(msg); + }; + + RCLCPP_INFO_STREAM(this->get_logger(), "Timer callback created for a standard IMU!"); + return cb; } else if constexpr (std::is_same::value) { // ------------------------------------------ // // ---------- Frame Pose call back ---------- // @@ -1227,7 +1351,91 @@ namespace obelisk { RCLCPP_INFO_STREAM(this->get_logger(), "Timer callback created for a Scan Dots (ObkScan) sensor!"); return cb; + } else if constexpr (std::is_same::value) { + // ------------------------------------------ // + // ------------ Depth Image Sensor ---------- // + // ------------------------------------------ // + auto cb = [publisher, sensor_names, mj_sensor_types, this]() { + std::lock_guard lock(sensor_data_mut_); + + std::string site = depth_scan_interface_->get_site(); + int site_id = mj_name2id(model_, mjOBJ_SITE, site.c_str()); + if (site_id == -1) { + throw std::runtime_error("Sensor not found in Mujoco! Make sure your XML has the site: " + site); + } + + // Site position and rotation in world frame + Eigen::Vector3d pos(data_->site_xpos[3*site_id + 0], data_->site_xpos[3*site_id + 1], data_->site_xpos[3*site_id + 2]); + Eigen::Matrix3d rot; + rot << data_->site_xmat[9*site_id + 0], data_->site_xmat[9*site_id + 1], data_->site_xmat[9*site_id + 2], + data_->site_xmat[9*site_id + 3], data_->site_xmat[9*site_id + 4], data_->site_xmat[9*site_id + 5], + data_->site_xmat[9*site_id + 6], data_->site_xmat[9*site_id + 7], data_->site_xmat[9*site_id + 8]; + + // Camera forward axis (site x-axis, first column of rotation matrix) + Eigen::Vector3d forward = rot.col(0); + + int img_w = depth_scan_interface_->get_image_width(); + int img_h = depth_scan_interface_->get_image_height(); + int num_rays = depth_scan_interface_->get_num_rays(); + + // Temp storage for geom id output + int geom_id[1] = { -1 }; + + obelisk::RayCasterInterface::MatrixX3d starts_w, dirs_w; + starts_w.resize(num_rays, 3); + dirs_w.resize(num_rays, 3); + + depth_scan_interface_->compute_rays_world(rot, pos, starts_w, dirs_w); + + // Build depth image buffer + std::vector depth_buffer(num_rays); + scan_viz_points_.clear(); + scan_viz_points_.reserve(num_rays); + + for (int ii = 0; ii < num_rays; ++ii) { + Eigen::Vector3d ray_origin = starts_w.row(ii).transpose(); + Eigen::Vector3d direction = dirs_w.row(ii).transpose(); + + // Perform ray cast + double dist = mj_ray(this->model_, this->data_, ray_origin.data(), direction.data(), depth_scan_interface_->get_geom_group_mask(), 1, -1, geom_id); + + if (dist < 0) { + // No hit - set to NaN + depth_buffer[ii] = std::numeric_limits::quiet_NaN(); + } else { + // Perpendicular depth: project ray distance onto camera forward axis + depth_buffer[ii] = static_cast(dist * direction.dot(forward)); + + // Store hit point for visualization + scan_viz_points_.push_back({ + ray_origin[0] + direction[0] * dist, + ray_origin[1] + direction[1] * dist, + ray_origin[2] + direction[2] * dist + }); + } + } + + // Build sensor_msgs::msg::Image (32FC1) + sensor_msgs::msg::Image msg; + msg.header.frame_id = "world"; + msg.header.stamp = this->now(); + msg.height = static_cast(img_h); + msg.width = static_cast(img_w); + msg.encoding = "32FC1"; + msg.is_bigendian = false; + msg.step = static_cast(img_w * sizeof(float)); + + size_t data_size = static_cast(msg.step) * img_h; + msg.data.resize(data_size); + std::memcpy(msg.data.data(), depth_buffer.data(), data_size); + + publisher->publish(msg); + }; + + RCLCPP_INFO_STREAM(this->get_logger(), "Timer callback created for a Depth Image (sensor_msgs::Image) sensor!"); + return cb; } + } /** @@ -1396,6 +1604,7 @@ namespace obelisk { // For the height map std::unique_ptr scan_interface_; + std::unique_ptr depth_scan_interface_; int scan_dots_idx_; // Constants diff --git a/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h b/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h index 2e0a8c63..baaf92f5 100644 --- a/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h +++ b/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h @@ -139,6 +139,9 @@ class LidarInterface : public RayCasterInterface { // All rays start from origin in local frame ray_starts_local_.setZero(); + nv_ = nv; + nh_ = nh; + // Meshgrid with indexing="ij" (outer: vertical, inner: horizontal) int idx = 0; for (int vi = 0; vi < nv; ++vi) { @@ -171,6 +174,13 @@ class LidarInterface : public RayCasterInterface { (void)hit_point; return dist; } + + int get_image_width() const override { return nh_; } + int get_image_height() const override { return nv_; } + + private: + int nv_ = 0; + int nh_ = 0; }; } // namespace obelisk diff --git a/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h b/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h index b2d28e39..0135f4e4 100644 --- a/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h +++ b/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h @@ -126,6 +126,9 @@ class RayCasterInterface { */ virtual float get_return(const std::array hit_point, const float dist) = 0; + virtual int get_image_width() const { return -1; } + virtual int get_image_height() const { return -1; } + protected: /** * @brief Offset transform (position and optional rotation) diff --git a/obelisk_ws/src/obelisk_ros/config/depth_config.yaml b/obelisk_ws/src/obelisk_ros/config/depth_config.yaml new file mode 100644 index 00000000..c9f3ed7d --- /dev/null +++ b/obelisk_ws/src/obelisk_ros/config/depth_config.yaml @@ -0,0 +1,12 @@ +site_name: "pelvis_mocap_site" +geom_groups: [0, 1, 2] + +pattern: + type: "lidar_scan" + vertical_fov_range: [-0., 50.] + horizontal_fov_range: [-30.0, 30.0] + channels: 26 + horizontal_res: 2.0 + offset: + pos: [0.153246, 0.0, 0.106799] + rot: [0.707107, 0, 0.707107, 0] diff --git a/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml index 979afe36..16f6a5e9 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml @@ -187,9 +187,15 @@ onboard: # config_path: /home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/height_scan_config.yaml # sensor_names: # scan_pos_sensor: framepos - - topic: /obelisk/g1/raycast - dt: 0.1 - msg_type: ObkScan + # - topic: /obelisk/g1/raycast + # dt: 0.1 + # msg_type: DepthImage + # config_path: /home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml + # sensor_names: + # scan_pos_sensor: framepos + - topic: /depth/image_processed + dt: 0.033 + msg_type: DepthImage config_path: /home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml sensor_names: scan_pos_sensor: framepos @@ -262,6 +268,20 @@ onboard: right_wrist_roll_joint_vel_sensor: jointvel right_wrist_pitch_joint_vel_sensor: jointvel right_wrist_yaw_joint_vel_sensor: jointvel + # ---------- Fast LIO ----------- # + - topic: /livox/imu + dt: 0.005 + msg_type: Imu + sensor_names: + lidar_imu_acc_sensor: accelerometer + lidar_imu_gyro_sensor: gyro + lidar_imu_quat_sensor: framequat + - topic: /Odometry + dt: 0.1 + msg_type: Odometry + sensor_names: + head_lidar_pos_sensor: framepos + head_lidar_orientation_sensor: framequat timers: - ros_parameter: timer_sensor_setting timer_period_sec: 0.02 diff --git a/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof.xml b/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof.xml index fccfaa14..3e944ce1 100644 --- a/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof.xml +++ b/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof.xml @@ -197,6 +197,7 @@ Position and velocity actuators added. --> + @@ -792,12 +793,12 @@ Position and velocity actuators added. --> - - - - - - + + + + + + From baef860dabac03a1de6c37abf9fde1f33eb40a79 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Tue, 17 Feb 2026 15:37:27 -0800 Subject: [PATCH 10/20] changed depth image computation --- .../include/obelisk_mujoco_sim_robot.h | 17 +++++++++++------ obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h | 15 +++++++++++++++ .../obelisk_cpp/lidar/ray_caster_interface.h | 1 + 3 files changed, 27 insertions(+), 6 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 index f200d803..ca7f5a01 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h @@ -1371,8 +1371,8 @@ namespace obelisk { data_->site_xmat[9*site_id + 3], data_->site_xmat[9*site_id + 4], data_->site_xmat[9*site_id + 5], data_->site_xmat[9*site_id + 6], data_->site_xmat[9*site_id + 7], data_->site_xmat[9*site_id + 8]; - // Camera forward axis (site x-axis, first column of rotation matrix) - Eigen::Vector3d forward = rot.col(0); + // Optical axis: center-of-FOV ray direction in world frame + Eigen::Vector3d forward = rot * depth_scan_interface_->get_image_forward_local(); int img_w = depth_scan_interface_->get_image_width(); int img_h = depth_scan_interface_->get_image_height(); @@ -1387,7 +1387,7 @@ namespace obelisk { depth_scan_interface_->compute_rays_world(rot, pos, starts_w, dirs_w); - // Build depth image buffer + // Build depth image buffer (rows flipped: vi=0 → bottom row, vi=nv-1 → top row) std::vector depth_buffer(num_rays); scan_viz_points_.clear(); scan_viz_points_.reserve(num_rays); @@ -1399,12 +1399,17 @@ namespace obelisk { // Perform ray cast double dist = mj_ray(this->model_, this->data_, ray_origin.data(), direction.data(), depth_scan_interface_->get_geom_group_mask(), 1, -1, geom_id); + // Flip rows for standard image convention (row 0 = top = highest elevation) + int vi = ii / img_w; + int hi = ii % img_w; + int out_idx = (img_h - 1 - vi) * img_w + hi; + if (dist < 0) { // No hit - set to NaN - depth_buffer[ii] = std::numeric_limits::quiet_NaN(); + depth_buffer[out_idx] = std::numeric_limits::quiet_NaN(); } else { - // Perpendicular depth: project ray distance onto camera forward axis - depth_buffer[ii] = static_cast(dist * direction.dot(forward)); + // Perpendicular depth: project ray distance onto optical axis + depth_buffer[out_idx] = static_cast(dist * direction.dot(forward)); // Store hit point for visualization scan_viz_points_.push_back({ diff --git a/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h b/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h index baaf92f5..4bf1736b 100644 --- a/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h +++ b/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h @@ -165,9 +165,22 @@ class LidarInterface : public RayCasterInterface { } } + // Compute the optical axis: center-of-FOV ray direction (before offset) + double vert_center_rad = (vert_min_deg + vert_max_deg) / 2.0 * M_PI / 180.0; + double horz_center_rad = (horz_min_deg + horz_max_deg) / 2.0 * M_PI / 180.0; + double cv_c = std::cos(vert_center_rad); + double sv_c = std::sin(vert_center_rad); + double ch_c = std::cos(horz_center_rad); + double sh_c = std::sin(horz_center_rad); + image_forward_local_ = Vector3d(cv_c * ch_c, cv_c * sh_c, sv_c); + // Apply offset transform (rotation to directions, position to starts) Offset offset = parse_offset(pattern); apply_offset(offset); + + // Apply the same offset rotation to the optical axis + image_forward_local_ = quat_apply(offset.rot, image_forward_local_); + image_forward_local_.normalize(); } float get_return(const std::array hit_point, const float dist) override { @@ -177,10 +190,12 @@ class LidarInterface : public RayCasterInterface { int get_image_width() const override { return nh_; } int get_image_height() const override { return nv_; } + Vector3d get_image_forward_local() const override { return image_forward_local_; } private: int nv_ = 0; int nh_ = 0; + Vector3d image_forward_local_ = Vector3d::Zero(); }; } // namespace obelisk diff --git a/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h b/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h index 0135f4e4..d340a1c7 100644 --- a/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h +++ b/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h @@ -128,6 +128,7 @@ class RayCasterInterface { virtual int get_image_width() const { return -1; } virtual int get_image_height() const { return -1; } + virtual Vector3d get_image_forward_local() const { return Vector3d::Zero(); } protected: /** From 724bd0aa049757594d2659000d53b6c59c39a04a Mon Sep 17 00:00:00 2001 From: Will Compton Date: Fri, 20 Feb 2026 22:00:34 -0800 Subject: [PATCH 11/20] depth camera pattern --- .claude/settings.local.json | 7 + .../include/obelisk_mujoco_sim_robot.h | 5 +- .../cpp/obelisk_cpp/lidar/depth_interface.h | 149 ++++++++++++++++++ .../src/obelisk_ros/config/depth_config.yaml | 11 +- .../src/obelisk_ros/config/g1_scan_cpp.yaml | 2 +- 5 files changed, 166 insertions(+), 8 deletions(-) create mode 100644 .claude/settings.local.json create mode 100644 obelisk/cpp/obelisk_cpp/lidar/depth_interface.h diff --git a/.claude/settings.local.json b/.claude/settings.local.json new file mode 100644 index 00000000..8a6946e7 --- /dev/null +++ b/.claude/settings.local.json @@ -0,0 +1,7 @@ +{ + "permissions": { + "allow": [ + "Read(//home/wcompton/repos/IsaacLab/source/isaaclab/isaaclab/sensors/ray_caster/patterns/**)" + ] + } +} diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h index ca7f5a01..b262d131 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h @@ -27,6 +27,7 @@ #include "obelisk_sensor_msgs/msg/obk_joint_encoders.hpp" #include "obelisk_sensor_msgs/msg/obk_scan.hpp" #include "obelisk_sim_robot.h" +#include "depth_interface.h" #include "height_scan_interface.h" #include "lidar_interface.h" @@ -640,10 +641,12 @@ namespace obelisk { const std::string type_str = type_node.as(); if (type_str == "lidar_scan") { depth_scan_interface_ = std::make_unique(sensor_config_path); + } else if (type_str == "depth_camera") { + depth_scan_interface_ = std::make_unique(sensor_config_path); } else { RCLCPP_ERROR_STREAM( this->get_logger(), - "DepthImage sensor type only supports 'lidar_scan' pattern, got: " + type_str + "DepthImage sensor type only supports 'lidar_scan' or 'depth_camera' pattern, got: " + type_str ); } diff --git a/obelisk/cpp/obelisk_cpp/lidar/depth_interface.h b/obelisk/cpp/obelisk_cpp/lidar/depth_interface.h new file mode 100644 index 00000000..b032de81 --- /dev/null +++ b/obelisk/cpp/obelisk_cpp/lidar/depth_interface.h @@ -0,0 +1,149 @@ +#pragma once + +#include "ray_caster_interface.h" + +namespace obelisk { + +/** + * @brief Pinhole camera depth ray caster + * + * Creates rays matching a pinhole camera model, mimicking Isaac Lab's + * PinholeCameraPatternCfg. Pixel coordinates are unprojected through the + * inverse intrinsic matrix, then transformed from camera frame (x-right, + * y-down, z-forward) to robotics frame (x-forward, y-left, z-up). + * Uses full 3D rotation for both ray starts and directions. + * + * YAML Config: + * site_name: "camera_site" # MuJoCo site name (required) + * geom_groups: [0] # Geom groups for collision (optional) + * pattern: + * width: 30 # Image width in pixels (required) + * height: 26 # Image height in pixels (required) + * intrinsic_matrix: [fx, 0, cx, # Row-major 3x3 intrinsic matrix (required) + * 0, fy, cy, + * 0, 0, 1] + * offset: # Offset transform applied in local frame (optional) + * pos: [0, 0, 0] # Position offset [x, y, z] + * rot: [1, 0, 0, 0] # Rotation quaternion (w, x, y, z) + */ +class DepthInterface : public RayCasterInterface { + public: + explicit DepthInterface(const std::string& yaml_path) : RayCasterInterface(yaml_path) { + setup_rays(get_config()); + } + + void compute_rays_world(const Matrix3d& site_xmat, const Vector3d& site_pos, + MatrixX3d& ray_starts_world, + MatrixX3d& ray_directions_world) const override { + // Full 3D rotation for both starts and directions + ray_starts_world = (site_xmat * ray_starts_local_.transpose()).transpose(); + ray_starts_world.rowwise() += site_pos.transpose(); + + ray_directions_world = (site_xmat * ray_directions_local_.transpose()).transpose(); + } + + protected: + void setup_rays(const YAML::Node& config) override { + auto pattern = config["pattern"]; + if (!pattern) { + throw std::runtime_error("Depth config missing 'pattern' section"); + } + + // Read image dimensions + if (!pattern["width"]) { + throw std::runtime_error("Depth config missing 'width'"); + } + if (!pattern["height"]) { + throw std::runtime_error("Depth config missing 'height'"); + } + width_ = pattern["width"].as(); + height_ = pattern["height"].as(); + if (width_ <= 0 || height_ <= 0) { + throw std::runtime_error("width and height must be > 0"); + } + + // Read intrinsic matrix (row-major, 9 elements: fx, 0, cx, 0, fy, cy, 0, 0, 1) + if (!pattern["intrinsic_matrix"]) { + throw std::runtime_error("Depth config missing 'intrinsic_matrix'"); + } + auto K_vec = pattern["intrinsic_matrix"].as>(); + if (K_vec.size() != 9) { + throw std::runtime_error("intrinsic_matrix must have exactly 9 elements"); + } + + // Build 3x3 intrinsic matrix and invert + Eigen::Matrix3d K; + K << K_vec[0], K_vec[1], K_vec[2], + K_vec[3], K_vec[4], K_vec[5], + K_vec[6], K_vec[7], K_vec[8]; + Eigen::Matrix3d K_inv = K.inverse(); + + int num_rays = width_ * height_; + ray_starts_local_.resize(num_rays, 3); + ray_directions_local_.resize(num_rays, 3); + + // All rays start from origin in local frame + ray_starts_local_.setZero(); + + // Generate pixel grid with "xy" indexing (outer loop over y/height, inner over x/width) + // and shift by +0.5 to center of pixel, matching Isaac Lab + int idx = 0; + for (int v = 0; v < height_; ++v) { + for (int u = 0; u < width_; ++u) { + // Pixel coordinates (centered) + double px = u + 0.5; + double py = v + 0.5; + + // Unproject through inverse intrinsic matrix + // cam_dir = K_inv * [px, py, 1]^T + Eigen::Vector3d pixel_h(px, py, 1.0); + Eigen::Vector3d cam_dir = K_inv * pixel_h; + + // Transform from camera frame (x-right, y-down, z-forward) + // to robotics frame (x-forward, y-left, z-up): + // robot_x = cam_z, robot_y = -cam_x, robot_z = -cam_y + Eigen::Vector3d robot_dir(cam_dir[2], -cam_dir[0], -cam_dir[1]); + + // Normalize to unit vector + robot_dir.normalize(); + + ray_directions_local_.row(idx) = robot_dir.transpose(); + idx++; + } + } + + // Optical axis: center pixel direction (before offset) + { + double cx = width_ / 2.0 + 0.5; + double cy = height_ / 2.0 + 0.5; + Eigen::Vector3d center_pixel(cx, cy, 1.0); + Eigen::Vector3d cam_center = K_inv * center_pixel; + image_forward_local_ = Vector3d(cam_center[2], -cam_center[0], -cam_center[1]); + image_forward_local_.normalize(); + } + + // Apply offset transform + Offset offset = parse_offset(pattern); + apply_offset(offset); + + // Apply the same offset rotation to the optical axis + image_forward_local_ = quat_apply(offset.rot, image_forward_local_); + image_forward_local_.normalize(); + } + + float get_return(const std::array hit_point, const float dist) override { + (void)hit_point; + return dist; + } + + int get_image_width() const override { return width_; } + int get_image_height() const override { return height_; } + Vector3d get_image_forward_local() const override { return image_forward_local_; } + + private: + int width_ = 0; + int height_ = 0; + Vector3d image_forward_local_ = Vector3d::Zero(); +}; + +} // namespace obelisk diff --git a/obelisk_ws/src/obelisk_ros/config/depth_config.yaml b/obelisk_ws/src/obelisk_ros/config/depth_config.yaml index c9f3ed7d..82cd54e7 100644 --- a/obelisk_ws/src/obelisk_ros/config/depth_config.yaml +++ b/obelisk_ws/src/obelisk_ros/config/depth_config.yaml @@ -2,11 +2,10 @@ site_name: "pelvis_mocap_site" geom_groups: [0, 1, 2] pattern: - type: "lidar_scan" - vertical_fov_range: [-0., 50.] - horizontal_fov_range: [-30.0, 30.0] - channels: 26 - horizontal_res: 2.0 + type: "depth_camera" + width: 30 + height: 26 + intrinsic_matrix: [25.9928, 0.0, 14.9936, 0.0, 27.8703, 12.9966, 0.0, 0.0, 1.0] offset: pos: [0.153246, 0.0, 0.106799] - rot: [0.707107, 0, 0.707107, 0] + rot: [0.843391, 0., 0.537300, 0.] diff --git a/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml index 16f6a5e9..80455c90 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml @@ -196,7 +196,7 @@ onboard: - topic: /depth/image_processed dt: 0.033 msg_type: DepthImage - config_path: /home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml + config_path: /home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/depth_config.yaml sensor_names: scan_pos_sensor: framepos - topic: /obelisk/g1/joint_encoders From 183d899c02b0c2364c63dc1f2314e64511f3cbca Mon Sep 17 00:00:00 2001 From: Will Compton Date: Sat, 21 Feb 2026 00:24:03 -0800 Subject: [PATCH 12/20] no flipping of the image --- .../obelisk_cpp/include/obelisk_mujoco_sim_robot.h | 12 ++++-------- 1 file changed, 4 insertions(+), 8 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 index b262d131..cd128b59 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h @@ -1390,7 +1390,8 @@ namespace obelisk { depth_scan_interface_->compute_rays_world(rot, pos, starts_w, dirs_w); - // Build depth image buffer (rows flipped: vi=0 → bottom row, vi=nv-1 → top row) + // Build depth image buffer in natural ray order (row 0 = top of image), + // matching DepthInterface iteration, Isaac Lab, and sensor_msgs/Image convention. std::vector depth_buffer(num_rays); scan_viz_points_.clear(); scan_viz_points_.reserve(num_rays); @@ -1402,17 +1403,12 @@ namespace obelisk { // Perform ray cast double dist = mj_ray(this->model_, this->data_, ray_origin.data(), direction.data(), depth_scan_interface_->get_geom_group_mask(), 1, -1, geom_id); - // Flip rows for standard image convention (row 0 = top = highest elevation) - int vi = ii / img_w; - int hi = ii % img_w; - int out_idx = (img_h - 1 - vi) * img_w + hi; - if (dist < 0) { // No hit - set to NaN - depth_buffer[out_idx] = std::numeric_limits::quiet_NaN(); + depth_buffer[ii] = std::numeric_limits::quiet_NaN(); } else { // Perpendicular depth: project ray distance onto optical axis - depth_buffer[out_idx] = static_cast(dist * direction.dot(forward)); + depth_buffer[ii] = static_cast(dist * direction.dot(forward)); // Store hit point for visualization scan_viz_points_.push_back({ From c24e0def8ff0bb90ada22ff7cdded8e136bda87a Mon Sep 17 00:00:00 2001 From: Will Compton Date: Tue, 3 Mar 2026 11:35:22 -0800 Subject: [PATCH 13/20] added PointCloud2 support --- .../include/obelisk_mujoco_sim_robot.h | 156 +++++++++++++++--- .../obelisk_cpp/lidar/ray_caster_interface.h | 11 ++ .../src/obelisk_ros/config/depth_config.yaml | 2 + .../src/obelisk_ros/config/g1_scan_cpp.yaml | 18 +- .../src/obelisk_ros/config/lidar_config.yaml | 18 +- .../robots/g1_description/mujoco/g1_27dof.xml | 2 +- 6 files changed, 166 insertions(+), 41 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 index cd128b59..868a7eda 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h @@ -21,6 +21,8 @@ #include #include #include +#include +#include #include "ament_index_cpp/get_package_share_directory.hpp" @@ -592,12 +594,7 @@ namespace obelisk { sensor_names, mj_sensor_types, this->template GetPublisher(sensor_key)), callback_group_); - } else if (sensor_type == "ObkScan") { - // Make a publisher and add it to the list - auto pub = ObeliskNode::create_publisher(topic, depth); - this->publishers_[sensor_key] = - std::make_shared>(pub); - + } else if ((sensor_type == "ObkScan") || (sensor_type == "PointCloud2")) { // Create the scanner interface // Parse the yaml YAML::Node scan_config = YAML::LoadFile(sensor_config_path); @@ -618,13 +615,34 @@ namespace obelisk { ); } + // Read viz_decimation from config (optional, default: 1) + if (scan_config["viz_decimation"]) { + scan_viz_decimation_ = std::max(1, scan_config["viz_decimation"].as()); + } + // Add the timer to the list - this->timers_[sensor_key] = this->create_wall_timer( - std::chrono::milliseconds(static_cast(1e3 * dt)), - CreateTimerCallback( - sensor_names, mj_sensor_types, - this->template GetPublisher(sensor_key)), - callback_group_); + if (sensor_type == "ObkScan") { + auto pub = ObeliskNode::create_publisher(topic, depth); + this->publishers_[sensor_key] = + std::make_shared>(pub); + this->timers_[sensor_key] = this->create_wall_timer( + std::chrono::milliseconds(static_cast(1e3 * dt)), + CreateTimerCallback( + sensor_names, mj_sensor_types, + this->template GetPublisher(sensor_key)), + callback_group_); + } else if (sensor_type == "PointCloud2") { + auto pub = ObeliskNode::create_publisher(topic, depth); + this->publishers_[sensor_key] = + std::make_shared>(pub); + this->timers_[sensor_key] = this->create_wall_timer( + std::chrono::milliseconds(static_cast(1e3 * dt)), + CreateTimerCallback( + sensor_names, mj_sensor_types, + this->template GetPublisher(sensor_key)), + callback_group_); + } + } else if (sensor_type == "DepthImage") { // Make a publisher and add it to the list auto pub = ObeliskNode::create_publisher(topic, depth); @@ -654,6 +672,11 @@ namespace obelisk { throw std::runtime_error("DepthImage sensor requires a scan interface with image dimensions"); } + // Read viz_decimation from config (optional, default: 1) + if (scan_config["viz_decimation"]) { + depth_viz_decimation_ = std::max(1, scan_config["viz_decimation"].as()); + } + // Add the timer to the list this->timers_[sensor_key] = this->create_wall_timer( std::chrono::milliseconds(static_cast(1e3 * dt)), @@ -1327,8 +1350,8 @@ namespace obelisk { scan_interface_->compute_rays_world(rot, pos, starts_w, dirs_w); scan_viz_points_.clear(); - scan_viz_points_.reserve(scan_interface_->get_num_rays()); - + scan_viz_points_.reserve(scan_interface_->get_num_rays() / scan_viz_decimation_ + 1); + for (int ii = 0; ii < scan_interface_->get_num_rays(); ++ii) { Eigen::Vector3d ray_origin = starts_w.row(ii).transpose(); Eigen::Vector3d direction = dirs_w.row(ii).transpose(); @@ -1344,16 +1367,99 @@ namespace obelisk { }; float ret = scan_interface_->get_return(hit_point, dist); msg.data.push_back(ret); - scan_viz_points_.push_back({hit_point[0], hit_point[1], hit_point[2]}); + if (ii % scan_viz_decimation_ == 0) { + scan_viz_points_.push_back({hit_point[0], hit_point[1], hit_point[2]}); + } } - msg.header.frame_id = "world"; + msg.header.frame_id = scan_interface_->get_frame(); msg.header.stamp = this->now(); publisher->publish(msg); }; RCLCPP_INFO_STREAM(this->get_logger(), "Timer callback created for a Scan Dots (ObkScan) sensor!"); return cb; + } else if constexpr (std::is_same::value) { + // ------------------------------------------ // + // ----------- PointCloud2 Sensor ----------- // + // ------------------------------------------ // + auto cb = [publisher, sensor_names, mj_sensor_types, this]() { + std::lock_guard lock(sensor_data_mut_); + + std::string site = scan_interface_->get_site(); + int site_id = mj_name2id(model_, mjOBJ_SITE, site.c_str()); + if (site_id == -1) { + throw std::runtime_error("Sensor not found in Mujoco! Make sure your XML has the site: " + scan_interface_->get_site()); + } + + // Site position and rotation in world frame + Eigen::Vector3d pos(data_->site_xpos[3*site_id + 0], data_->site_xpos[3*site_id + 1], data_->site_xpos[3*site_id + 2]); + Eigen::Matrix3d rot; + rot << data_->site_xmat[9*site_id + 0], data_->site_xmat[9*site_id + 1], data_->site_xmat[9*site_id + 2], + data_->site_xmat[9*site_id + 3], data_->site_xmat[9*site_id + 4], data_->site_xmat[9*site_id + 5], + data_->site_xmat[9*site_id + 6], data_->site_xmat[9*site_id + 7], data_->site_xmat[9*site_id + 8]; + + // Temp storage for geom id output + int geom_id[1] = { -1 }; + + obelisk::RayCasterInterface::MatrixX3d starts_w, dirs_w; + int num_rays = scan_interface_->get_num_rays(); + starts_w.resize(num_rays, 3); + dirs_w.resize(num_rays, 3); + + scan_interface_->compute_rays_world(rot, pos, starts_w, dirs_w); + + // Collect hit points + std::vector> points; + points.reserve(num_rays); + scan_viz_points_.clear(); + scan_viz_points_.reserve(num_rays / scan_viz_decimation_ + 1); + + for (int ii = 0; ii < num_rays; ++ii) { + Eigen::Vector3d ray_origin = starts_w.row(ii).transpose(); + Eigen::Vector3d direction = dirs_w.row(ii).transpose(); + + double dist = mj_ray(this->model_, this->data_, ray_origin.data(), direction.data(), scan_interface_->get_geom_group_mask(), 1, -1, geom_id); + + if (dist >= 0) { + float hx = static_cast(ray_origin[0] + direction[0] * dist); + float hy = static_cast(ray_origin[1] + direction[1] * dist); + float hz = static_cast(ray_origin[2] + direction[2] * dist); + points.push_back({hx, hy, hz}); + if (ii % scan_viz_decimation_ == 0) { + scan_viz_points_.push_back({static_cast(hx), static_cast(hy), static_cast(hz)}); + } + } + } + + // Build PointCloud2 message + sensor_msgs::msg::PointCloud2 msg; + msg.header.frame_id = scan_interface_->get_frame(); + msg.header.stamp = this->now(); + msg.height = 1; + msg.width = static_cast(points.size()); + msg.is_dense = true; + msg.is_bigendian = false; + + sensor_msgs::PointCloud2Modifier modifier(msg); + modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.resize(points.size()); + + sensor_msgs::PointCloud2Iterator iter_x(msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(msg, "z"); + + for (const auto& pt : points) { + *iter_x = pt[0]; ++iter_x; + *iter_y = pt[1]; ++iter_y; + *iter_z = pt[2]; ++iter_z; + } + + publisher->publish(msg); + }; + + RCLCPP_INFO_STREAM(this->get_logger(), "Timer callback created for a PointCloud2 sensor!"); + return cb; } else if constexpr (std::is_same::value) { // ------------------------------------------ // // ------------ Depth Image Sensor ---------- // @@ -1394,7 +1500,7 @@ namespace obelisk { // matching DepthInterface iteration, Isaac Lab, and sensor_msgs/Image convention. std::vector depth_buffer(num_rays); scan_viz_points_.clear(); - scan_viz_points_.reserve(num_rays); + scan_viz_points_.reserve(num_rays / depth_viz_decimation_ + 1); for (int ii = 0; ii < num_rays; ++ii) { Eigen::Vector3d ray_origin = starts_w.row(ii).transpose(); @@ -1411,17 +1517,19 @@ namespace obelisk { depth_buffer[ii] = static_cast(dist * direction.dot(forward)); // Store hit point for visualization - scan_viz_points_.push_back({ - ray_origin[0] + direction[0] * dist, - ray_origin[1] + direction[1] * dist, - ray_origin[2] + direction[2] * dist - }); + if (ii % depth_viz_decimation_ == 0) { + scan_viz_points_.push_back({ + ray_origin[0] + direction[0] * dist, + ray_origin[1] + direction[1] * dist, + ray_origin[2] + direction[2] * dist + }); + } } } // Build sensor_msgs::msg::Image (32FC1) sensor_msgs::msg::Image msg; - msg.header.frame_id = "world"; + msg.header.frame_id = depth_scan_interface_->get_frame(); msg.header.stamp = this->now(); msg.height = static_cast(img_h); msg.width = static_cast(img_w); @@ -1609,6 +1717,8 @@ namespace obelisk { // For the height map std::unique_ptr scan_interface_; std::unique_ptr depth_scan_interface_; + int scan_viz_decimation_ = 1; + int depth_viz_decimation_ = 1; int scan_dots_idx_; // Constants diff --git a/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h b/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h index d340a1c7..ffda486a 100644 --- a/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h +++ b/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h @@ -71,6 +71,11 @@ class RayCasterInterface { } else { geom_group_mask_[0] = 1; } + + // Read frame (optional, default: "world") + if (config_["frame"]) { + frame_ = config_["frame"].as(); + } } virtual ~RayCasterInterface() = default; @@ -108,6 +113,11 @@ class RayCasterInterface { */ const mjtByte* get_geom_group_mask() const { return geom_group_mask_; } + /** + * @brief Get the frame_id string for published messages + */ + const std::string& get_frame() const { return frame_; } + /** * @brief Compute ray starts and directions in world frame * @param site_xmat 3x3 rotation matrix from site orientation @@ -209,6 +219,7 @@ class RayCasterInterface { YAML::Node config_; std::string site_name_; mjtByte geom_group_mask_[mjNGROUP]; + std::string frame_ = "world"; }; } // namespace obelisk diff --git a/obelisk_ws/src/obelisk_ros/config/depth_config.yaml b/obelisk_ws/src/obelisk_ros/config/depth_config.yaml index 82cd54e7..e7fc4af3 100644 --- a/obelisk_ws/src/obelisk_ros/config/depth_config.yaml +++ b/obelisk_ws/src/obelisk_ros/config/depth_config.yaml @@ -1,5 +1,7 @@ site_name: "pelvis_mocap_site" geom_groups: [0, 1, 2] +viz_decimation: 1 +frame: "world" pattern: type: "depth_camera" diff --git a/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml b/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml index 80455c90..45bd99f6 100644 --- a/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml +++ b/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml @@ -187,18 +187,18 @@ onboard: # config_path: /home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/height_scan_config.yaml # sensor_names: # scan_pos_sensor: framepos - # - topic: /obelisk/g1/raycast - # dt: 0.1 + - topic: /cloud_registered_body + dt: 0.1 + msg_type: PointCloud2 + config_path: /home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml + sensor_names: + scan_pos_sensor: framepos + # - topic: /depth/image_processed + # dt: 0.033 # msg_type: DepthImage - # config_path: /home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml + # config_path: /home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/depth_config.yaml # sensor_names: # scan_pos_sensor: framepos - - topic: /depth/image_processed - dt: 0.033 - msg_type: DepthImage - config_path: /home/wcompton/repos/obelisk/obelisk_ws/src/obelisk_ros/config/depth_config.yaml - sensor_names: - scan_pos_sensor: framepos - topic: /obelisk/g1/joint_encoders dt: 0.002 msg_type: ObkJointEncoders diff --git a/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml b/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml index af74f7b6..38558002 100644 --- a/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml +++ b/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml @@ -1,12 +1,14 @@ -site_name: "pelvis_mocap_site" -geom_groups: [0, 1, 2] +site_name: "head_lidar_site" +geom_groups: [2] +viz_decimation: 10 +frame: "body" pattern: type: "lidar_scan" - vertical_fov_range: [-25.0, 25.0] - horizontal_fov_range: [-25.0, 25.0] - channels: 21 - horizontal_res: 2.5 + vertical_fov_range: [0.0, 51.0] + horizontal_fov_range: [0., 360.] + channels: 51 + horizontal_res: 1.0 offset: - pos: [0.08, 0.0, 0.15] - rot: [0.8660, 0.0, 0.5000, 0.0] + pos: [0., 0., 0.] + rot: [1., 0., 0., 0.] diff --git a/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof.xml b/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof.xml index 3e944ce1..63decc4d 100644 --- a/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof.xml +++ b/obelisk_ws/src/robots/g1_description/mujoco/g1_27dof.xml @@ -376,7 +376,7 @@ Position and velocity actuators added. --> - + From 8a1072466458583d2f7eda0045ad2343c00c27e3 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Tue, 3 Mar 2026 14:20:48 -0800 Subject: [PATCH 14/20] PCL2 to body frame --- .../include/obelisk_mujoco_sim_robot.h | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 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 index 122c0823..943da5d0 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h @@ -1410,6 +1410,10 @@ namespace obelisk { scan_interface_->compute_rays_world(rot, pos, starts_w, dirs_w); + // Local-frame rays for body-frame point cloud + const auto& starts_l = scan_interface_->get_ray_starts_local(); + const auto& dirs_l = scan_interface_->get_ray_directions_local(); + // Collect hit points std::vector> points; points.reserve(num_rays); @@ -1423,12 +1427,20 @@ namespace obelisk { double dist = mj_ray(this->model_, this->data_, ray_origin.data(), direction.data(), scan_interface_->get_geom_group_mask(), 1, -1, geom_id); if (dist >= 0) { - float hx = static_cast(ray_origin[0] + direction[0] * dist); - float hy = static_cast(ray_origin[1] + direction[1] * dist); - float hz = static_cast(ray_origin[2] + direction[2] * dist); + // Compute point in sensor body frame for the point cloud + Eigen::Vector3d ray_origin_local = starts_l.row(ii).transpose(); + Eigen::Vector3d direction_local = dirs_l.row(ii).transpose(); + float hx = static_cast(ray_origin_local[0] + direction_local[0] * dist); + float hy = static_cast(ray_origin_local[1] + direction_local[1] * dist); + float hz = static_cast(ray_origin_local[2] + direction_local[2] * dist); points.push_back({hx, hy, hz}); + + // Viz points remain in world frame if (ii % scan_viz_decimation_ == 0) { - scan_viz_points_.push_back({static_cast(hx), static_cast(hy), static_cast(hz)}); + double wx = ray_origin[0] + direction[0] * dist; + double wy = ray_origin[1] + direction[1] * dist; + double wz = ray_origin[2] + direction[2] * dist; + scan_viz_points_.push_back({wx, wy, wz}); } } } From 3e71943f808d54df400036d1ff6f51067993efba Mon Sep 17 00:00:00 2001 From: Zach Olkin Date: Fri, 27 Mar 2026 16:28:42 -0700 Subject: [PATCH 15/20] removed the foxglove bridge because it was dropped from humble for some reason --- scripts/install_sys_deps.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/install_sys_deps.sh b/scripts/install_sys_deps.sh index e6fa4c7c..7545b11d 100644 --- a/scripts/install_sys_deps.sh +++ b/scripts/install_sys_deps.sh @@ -87,7 +87,7 @@ if [ "$basic" = true ]; then ros-humble-rosidl-default-generators \ ros-humble-rmw-cyclonedds-cpp \ ros-humble-rviz-visual-tools \ - ros-humble-foxglove-bridge \ + # ros-humble-foxglove-bridge \ # Dropped from humble for some reason. ros-humble-joy # Need to make sure this is version 3.3.0 source /opt/ros/humble/setup.bash From d05dd265d37804b53be861e58f6df007874a8b51 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Thu, 16 Apr 2026 14:22:54 -0700 Subject: [PATCH 16/20] viz flag for scans --- .../include/obelisk_mujoco_sim_robot.h | 78 +++++++++++++------ 1 file changed, 53 insertions(+), 25 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 index 943da5d0..0bd28f46 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h @@ -10,6 +10,7 @@ #include #include #include +#include #include #include @@ -312,12 +313,14 @@ namespace obelisk { mjtNum mat[9] = {1,0,0, 0,1,0, 0,0,1}; float rgba[4] = {1,0,0,1}; - for (auto &p : scan_viz_points_) { - if (scn.ngeom >= scn.maxgeom) break; + for (auto &kv : scan_viz_points_) { + for (auto &p : kv.second) { + if (scn.ngeom >= scn.maxgeom) break; - mjvGeom g; - mjv_initGeom(&g, mjGEOM_SPHERE, size, p.data(), mat, rgba); - scn.geoms[scn.ngeom++] = g; + mjvGeom g; + mjv_initGeom(&g, mjGEOM_SPHERE, size, p.data(), mat, rgba); + scn.geoms[scn.ngeom++] = g; + } } } @@ -621,6 +624,11 @@ namespace obelisk { scan_viz_decimation_ = std::max(1, scan_config["viz_decimation"].as()); } + // Read viz enable flag from config (optional, default: true) + if (scan_config["viz"]) { + scan_viz_enabled_ = scan_config["viz"].as(); + } + // Add the timer to the list if (sensor_type == "ObkScan") { auto pub = ObeliskNode::create_publisher(topic, depth); @@ -630,7 +638,8 @@ namespace obelisk { std::chrono::milliseconds(static_cast(1e3 * dt)), CreateTimerCallback( sensor_names, mj_sensor_types, - this->template GetPublisher(sensor_key)), + this->template GetPublisher(sensor_key), + sensor_key), callback_group_); } else if (sensor_type == "PointCloud2") { auto pub = ObeliskNode::create_publisher(topic, depth); @@ -640,7 +649,8 @@ namespace obelisk { std::chrono::milliseconds(static_cast(1e3 * dt)), CreateTimerCallback( sensor_names, mj_sensor_types, - this->template GetPublisher(sensor_key)), + this->template GetPublisher(sensor_key), + sensor_key), callback_group_); } @@ -678,12 +688,18 @@ namespace obelisk { depth_viz_decimation_ = std::max(1, scan_config["viz_decimation"].as()); } + // Read viz enable flag from config (optional, default: true) + if (scan_config["viz"]) { + depth_viz_enabled_ = scan_config["viz"].as(); + } + // Add the timer to the list this->timers_[sensor_key] = this->create_wall_timer( std::chrono::milliseconds(static_cast(1e3 * dt)), CreateTimerCallback( sensor_names, mj_sensor_types, - this->template GetPublisher(sensor_key)), + this->template GetPublisher(sensor_key), + sensor_key), callback_group_); } else { throw std::runtime_error("Sensor type not supported: " + sensor_type); @@ -877,7 +893,8 @@ namespace obelisk { std::function CreateTimerCallback(const std::vector& sensor_names, const std::vector& mj_sensor_types, - std::shared_ptr> publisher) { + std::shared_ptr> publisher, + const std::string& sensor_key = "") { if constexpr (std::is_same::value) { // Check that joints with encoders are only hinge or slide. for (size_t i = 0; i < sensor_names.size(); i++) { @@ -1321,7 +1338,7 @@ namespace obelisk { // ------------------------------------------ // // ------------ Scan Dots Sensor ------------ // // ------------------------------------------ // - auto cb = [publisher, sensor_names, mj_sensor_types, this]() { + auto cb = [publisher, sensor_names, mj_sensor_types, sensor_key, this]() { // This sensor is always made up of: // - Framepos std::lock_guard lock(sensor_data_mut_); @@ -1350,8 +1367,11 @@ namespace obelisk { scan_interface_->compute_rays_world(rot, pos, starts_w, dirs_w); - scan_viz_points_.clear(); - scan_viz_points_.reserve(scan_interface_->get_num_rays() / scan_viz_decimation_ + 1); + auto& viz_bucket = scan_viz_points_[sensor_key]; + if (scan_viz_enabled_) { + viz_bucket.clear(); + viz_bucket.reserve(scan_interface_->get_num_rays() / scan_viz_decimation_ + 1); + } for (int ii = 0; ii < scan_interface_->get_num_rays(); ++ii) { Eigen::Vector3d ray_origin = starts_w.row(ii).transpose(); @@ -1368,8 +1388,8 @@ namespace obelisk { }; float ret = scan_interface_->get_return(hit_point, dist); msg.data.push_back(ret); - if (ii % scan_viz_decimation_ == 0) { - scan_viz_points_.push_back({hit_point[0], hit_point[1], hit_point[2]}); + if (scan_viz_enabled_ && ii % scan_viz_decimation_ == 0) { + viz_bucket.push_back({hit_point[0], hit_point[1], hit_point[2]}); } } @@ -1384,7 +1404,7 @@ namespace obelisk { // ------------------------------------------ // // ----------- PointCloud2 Sensor ----------- // // ------------------------------------------ // - auto cb = [publisher, sensor_names, mj_sensor_types, this]() { + auto cb = [publisher, sensor_names, mj_sensor_types, sensor_key, this]() { std::lock_guard lock(sensor_data_mut_); std::string site = scan_interface_->get_site(); @@ -1417,8 +1437,11 @@ namespace obelisk { // Collect hit points std::vector> points; points.reserve(num_rays); - scan_viz_points_.clear(); - scan_viz_points_.reserve(num_rays / scan_viz_decimation_ + 1); + auto& viz_bucket = scan_viz_points_[sensor_key]; + if (scan_viz_enabled_) { + viz_bucket.clear(); + viz_bucket.reserve(num_rays / scan_viz_decimation_ + 1); + } for (int ii = 0; ii < num_rays; ++ii) { Eigen::Vector3d ray_origin = starts_w.row(ii).transpose(); @@ -1436,11 +1459,11 @@ namespace obelisk { points.push_back({hx, hy, hz}); // Viz points remain in world frame - if (ii % scan_viz_decimation_ == 0) { + if (scan_viz_enabled_ && ii % scan_viz_decimation_ == 0) { double wx = ray_origin[0] + direction[0] * dist; double wy = ray_origin[1] + direction[1] * dist; double wz = ray_origin[2] + direction[2] * dist; - scan_viz_points_.push_back({wx, wy, wz}); + viz_bucket.push_back({wx, wy, wz}); } } } @@ -1477,7 +1500,7 @@ namespace obelisk { // ------------------------------------------ // // ------------ Depth Image Sensor ---------- // // ------------------------------------------ // - auto cb = [publisher, sensor_names, mj_sensor_types, this]() { + auto cb = [publisher, sensor_names, mj_sensor_types, sensor_key, this]() { std::lock_guard lock(sensor_data_mut_); std::string site = depth_scan_interface_->get_site(); @@ -1512,8 +1535,11 @@ namespace obelisk { // Build depth image buffer in natural ray order (row 0 = top of image), // matching DepthInterface iteration, Isaac Lab, and sensor_msgs/Image convention. std::vector depth_buffer(num_rays); - scan_viz_points_.clear(); - scan_viz_points_.reserve(num_rays / depth_viz_decimation_ + 1); + auto& viz_bucket = scan_viz_points_[sensor_key]; + if (depth_viz_enabled_) { + viz_bucket.clear(); + viz_bucket.reserve(num_rays / depth_viz_decimation_ + 1); + } for (int ii = 0; ii < num_rays; ++ii) { Eigen::Vector3d ray_origin = starts_w.row(ii).transpose(); @@ -1530,8 +1556,8 @@ namespace obelisk { depth_buffer[ii] = static_cast(dist * direction.dot(forward)); // Store hit point for visualization - if (ii % depth_viz_decimation_ == 0) { - scan_viz_points_.push_back({ + if (depth_viz_enabled_ && ii % depth_viz_decimation_ == 0) { + viz_bucket.push_back({ ray_origin[0] + direction[0] * dist, ray_origin[1] + direction[1] * dist, ray_origin[2] + direction[2] * dist @@ -1708,7 +1734,7 @@ namespace obelisk { // Geom id's for viz std::vector viz_geoms_; - std::vector> scan_viz_points_; + std::unordered_map>> scan_viz_points_; // Shared data between the main thread and the sim thread std::vector shared_data_; @@ -1732,6 +1758,8 @@ namespace obelisk { std::unique_ptr depth_scan_interface_; int scan_viz_decimation_ = 1; int depth_viz_decimation_ = 1; + bool scan_viz_enabled_ = true; + bool depth_viz_enabled_ = true; int scan_dots_idx_; // Constants From b5307bf8d30d521e405f9d553da893c7e56a7706 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Thu, 16 Apr 2026 16:22:58 -0700 Subject: [PATCH 17/20] maximum range for scans --- .../include/obelisk_mujoco_sim_robot.h | 3 ++ .../cpp/obelisk_cpp/lidar/depth_interface.h | 1 + .../obelisk_cpp/lidar/height_scan_interface.h | 1 + .../cpp/obelisk_cpp/lidar/lidar_interface.h | 1 + .../obelisk_cpp/lidar/ray_caster_interface.h | 29 +++++++++++++++++++ 5 files changed, 35 insertions(+) diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h index 0bd28f46..24c5f5d4 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h @@ -1379,6 +1379,7 @@ namespace obelisk { // Perform ray cast double dist = mj_ray(this->model_, this->data_, ray_origin.data(), direction.data(), scan_interface_->get_geom_group_mask(), 1, -1, geom_id); + dist = scan_interface_->apply_max_distance(dist); // Compute hit point std::array hit_point = { @@ -1448,6 +1449,7 @@ namespace obelisk { Eigen::Vector3d direction = dirs_w.row(ii).transpose(); double dist = mj_ray(this->model_, this->data_, ray_origin.data(), direction.data(), scan_interface_->get_geom_group_mask(), 1, -1, geom_id); + dist = scan_interface_->apply_max_distance(dist); if (dist >= 0) { // Compute point in sensor body frame for the point cloud @@ -1547,6 +1549,7 @@ namespace obelisk { // Perform ray cast double dist = mj_ray(this->model_, this->data_, ray_origin.data(), direction.data(), depth_scan_interface_->get_geom_group_mask(), 1, -1, geom_id); + dist = depth_scan_interface_->apply_max_distance(dist); if (dist < 0) { // No hit - set to NaN diff --git a/obelisk/cpp/obelisk_cpp/lidar/depth_interface.h b/obelisk/cpp/obelisk_cpp/lidar/depth_interface.h index b032de81..e6456cba 100644 --- a/obelisk/cpp/obelisk_cpp/lidar/depth_interface.h +++ b/obelisk/cpp/obelisk_cpp/lidar/depth_interface.h @@ -16,6 +16,7 @@ namespace obelisk { * YAML Config: * site_name: "camera_site" # MuJoCo site name (required) * geom_groups: [0] # Geom groups for collision (optional) + * max_distance: 0.0 # Max ray distance in meters, 0 disables (optional) * pattern: * width: 30 # Image width in pixels (required) * height: 26 # Image height in pixels (required) diff --git a/obelisk/cpp/obelisk_cpp/lidar/height_scan_interface.h b/obelisk/cpp/obelisk_cpp/lidar/height_scan_interface.h index 062c1bb8..54856230 100644 --- a/obelisk/cpp/obelisk_cpp/lidar/height_scan_interface.h +++ b/obelisk/cpp/obelisk_cpp/lidar/height_scan_interface.h @@ -14,6 +14,7 @@ namespace obelisk { * YAML Config: * site_name: "height_scan_site" # MuJoCo site name (required) * geom_groups: [0] # Geom groups for collision (optional) + * max_distance: 0.0 # Max ray distance in meters, 0 disables (optional) * pattern: * ordering: "xy" # Grid ordering: "xy" or "yx" (optional, default: "xy") * resolution: 0.05 # Grid step size in meters (required) diff --git a/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h b/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h index 4bf1736b..428288ee 100644 --- a/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h +++ b/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h @@ -18,6 +18,7 @@ namespace obelisk { * YAML Config: * site_name: "lidar_site" # MuJoCo site name (required) * geom_groups: [0] # Geom groups for collision (optional) + * max_distance: 0.0 # Max ray distance in meters, 0 disables (optional) * pattern: * vertical_fov_range: [-15, 15] # [min, max] degrees (required) * horizontal_fov_range: [0, 360] # [min, max] degrees (required) diff --git a/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h b/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h index ffda486a..61e081b0 100644 --- a/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h +++ b/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h @@ -76,6 +76,15 @@ class RayCasterInterface { if (config_["frame"]) { frame_ = config_["frame"].as(); } + + // Read max ray distance (optional, default: 0.0 meaning "disabled") + // Hits beyond this distance are treated as no-hit (apply_max_distance returns -1). + if (config_["max_distance"]) { + max_distance_ = config_["max_distance"].as(); + if (max_distance_ < 0.0) { + throw std::runtime_error("max_distance must be >= 0"); + } + } } virtual ~RayCasterInterface() = default; @@ -118,6 +127,25 @@ class RayCasterInterface { */ const std::string& get_frame() const { return frame_; } + /** + * @brief Max ray distance in meters. 0.0 means disabled (no limit). + */ + double get_max_distance() const { return max_distance_; } + + /** + * @brief Clamp a raycast distance by max_distance. + * + * If max_distance is configured (>0) and the hit is farther than the limit, + * returns -1 (the mj_ray no-hit sentinel) so callers can treat it as a miss. + * Otherwise returns dist unchanged (including the existing -1 no-hit case). + */ + double apply_max_distance(double dist) const { + if (max_distance_ > 0.0 && dist > max_distance_) { + return -1.0; + } + return dist; + } + /** * @brief Compute ray starts and directions in world frame * @param site_xmat 3x3 rotation matrix from site orientation @@ -220,6 +248,7 @@ class RayCasterInterface { std::string site_name_; mjtByte geom_group_mask_[mjNGROUP]; std::string frame_ = "world"; + double max_distance_ = 0.0; // 0 => disabled }; } // namespace obelisk From 60b90f06de37dca49847ccdc2dbc65d8229a477f Mon Sep 17 00:00:00 2001 From: Will Compton Date: Thu, 16 Apr 2026 17:05:04 -0700 Subject: [PATCH 18/20] fill intensity field for fast-lio --- .../cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h index 24c5f5d4..1303d08d 100644 --- a/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h +++ b/obelisk/cpp/obelisk_cpp/include/obelisk_mujoco_sim_robot.h @@ -1480,17 +1480,24 @@ namespace obelisk { msg.is_bigendian = false; sensor_msgs::PointCloud2Modifier modifier(msg); - modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.setPointCloud2Fields(4, + "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); modifier.resize(points.size()); sensor_msgs::PointCloud2Iterator iter_x(msg, "x"); sensor_msgs::PointCloud2Iterator iter_y(msg, "y"); sensor_msgs::PointCloud2Iterator iter_z(msg, "z"); + sensor_msgs::PointCloud2Iterator iter_i(msg, "intensity"); + constexpr float kDefaultIntensity = 128.0f; for (const auto& pt : points) { *iter_x = pt[0]; ++iter_x; *iter_y = pt[1]; ++iter_y; *iter_z = pt[2]; ++iter_z; + *iter_i = kDefaultIntensity; ++iter_i; } publisher->publish(msg); From e44aff087a46ceddd1dcf908facbeb9d9cb40337 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Sat, 18 Apr 2026 13:55:17 -0700 Subject: [PATCH 19/20] fix parsing bug for ros joy package --- scripts/install_sys_deps.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/install_sys_deps.sh b/scripts/install_sys_deps.sh index 7545b11d..4a5aef46 100644 --- a/scripts/install_sys_deps.sh +++ b/scripts/install_sys_deps.sh @@ -87,8 +87,8 @@ if [ "$basic" = true ]; then ros-humble-rosidl-default-generators \ ros-humble-rmw-cyclonedds-cpp \ ros-humble-rviz-visual-tools \ + ros-humble-joy \ # Need to make sure this is version 3.3.0 # ros-humble-foxglove-bridge \ # Dropped from humble for some reason. - ros-humble-joy # Need to make sure this is version 3.3.0 source /opt/ros/humble/setup.bash # python deps From 0319a7955b6131ff8b9ed3f1397ba62c12e8ded1 Mon Sep 17 00:00:00 2001 From: Will Compton Date: Sat, 18 Apr 2026 13:57:36 -0700 Subject: [PATCH 20/20] fix parsing bug for ros joy package --- scripts/install_sys_deps.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/install_sys_deps.sh b/scripts/install_sys_deps.sh index 4a5aef46..84266a3c 100644 --- a/scripts/install_sys_deps.sh +++ b/scripts/install_sys_deps.sh @@ -87,8 +87,8 @@ if [ "$basic" = true ]; then ros-humble-rosidl-default-generators \ ros-humble-rmw-cyclonedds-cpp \ ros-humble-rviz-visual-tools \ - ros-humble-joy \ # Need to make sure this is version 3.3.0 - # ros-humble-foxglove-bridge \ # Dropped from humble for some reason. + ros-humble-joy + # NOTE: ros-humble-foxglove-bridge Dropped from humble for some reason. source /opt/ros/humble/setup.bash # python deps