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/hardware/robots/unitree/g1_interface.h b/obelisk/cpp/hardware/robots/unitree/g1_interface.h index 62f34ad5..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++; } @@ -253,7 +264,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 +279,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 100c1803..a1dd7cf2 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( @@ -88,10 +89,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,11 +100,15 @@ 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)); } + + // 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( @@ -117,6 +122,10 @@ namespace obelisk { ReleaseUnitreeMotionControl(); } + if (logging_) { + InitializeLogging(); + } + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -301,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/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..1303d08d 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,10 @@ #include #include +#include #include +#include +#include #include #include @@ -16,12 +19,21 @@ #include #include #include +#include #include +#include +#include +#include +#include #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 "depth_interface.h" +#include "height_scan_interface.h" +#include "lidar_interface.h" namespace obelisk { template class ObeliskMujocoRobot : public ObeliskSimRobot { @@ -74,13 +86,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")); @@ -303,6 +308,21 @@ 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 &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; + } + } + } mjr_render(viewport, &scn, &con); @@ -478,6 +498,13 @@ 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 = "&"; @@ -519,18 +546,31 @@ 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); + 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 == "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 @@ -558,21 +598,111 @@ namespace obelisk { sensor_names, mj_sensor_types, this->template GetPublisher(sensor_key)), callback_group_); - } else if (sensor_type == "GridCells") { + } else if ((sensor_type == "ObkScan") || (sensor_type == "PointCloud2")) { + // 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'" + ); + } + + // Read viz_decimation from config (optional, default: 1) + if (scan_config["viz_decimation"]) { + 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); + 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), + 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), + 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); + auto pub = ObeliskNode::create_publisher(topic, depth); this->publishers_[sensor_key] = - std::make_shared>(pub); + 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 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' or 'depth_camera' 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"); + } + + // Read viz_decimation from config (optional, default: 1) + if (scan_config["viz_decimation"]) { + 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( + 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!"); + throw std::runtime_error("Sensor type not supported: " + sensor_type); } this->timers_[sensor_key]->cancel(); // Stop the timer @@ -763,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++) { @@ -827,12 +958,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. @@ -908,6 +1039,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 ---------- // @@ -1130,183 +1334,269 @@ 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]() { + 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_); - nav_msgs::msg::GridCells msg; + 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()); + } - 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!"); + // 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); + + 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); } - 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!"); + 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); + dist = scan_interface_->apply_max_distance(dist); + + // 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); + if (scan_viz_enabled_ && ii % scan_viz_decimation_ == 0) { + viz_bucket.push_back({hit_point[0], hit_point[1], hit_point[2]}); + } } + msg.header.frame_id = scan_interface_->get_frame(); + msg.header.stamp = this->now(); + publisher->publish(msg); + }; - std::array x_y_num_rays = {x_rays, y_rays}; + 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, sensor_key, this]() { + std::lock_guard lock(sensor_data_mut_); - msg.cell_width = this->height_map_grid_spacing_; + 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()); + } - 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)); + // 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); + + // 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); + 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(); + 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 + 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 (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; + viz_bucket.push_back({wx, wy, wz}); + } } - // 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; - } + // 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.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; + } - // 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++; - } - } + publisher->publish(msg); + }; - 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)); - } + 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 ---------- // + // ------------------------------------------ // + 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(); + 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]; + + // 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(); + 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 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); + 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(); + 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); + dist = depth_scan_interface_->apply_max_distance(dist); + + if (dist < 0) { + // No hit - set to NaN + depth_buffer[ii] = std::numeric_limits::quiet_NaN(); } 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)); + // Perpendicular depth: project ray distance onto optical axis + depth_buffer[ii] = static_cast(dist * direction.dot(forward)); + + // Store hit point for visualization + 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 + }); + } } - - msg.header.stamp = this->now(); } + + // Build sensor_msgs::msg::Image (32FC1) + sensor_msgs::msg::Image msg; + 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); + 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 Scan Dots (GridCells) sensor!"); + RCLCPP_INFO_STREAM(this->get_logger(), "Timer callback created for a Depth Image (sensor_msgs::Image) sensor!"); return cb; } + } /** @@ -1454,6 +1744,7 @@ namespace obelisk { // Geom id's for viz std::vector viz_geoms_; + std::unordered_map>> scan_viz_points_; // Shared data between the main thread and the sim thread std::vector shared_data_; @@ -1473,9 +1764,13 @@ 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_; + 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 static constexpr float TIME_STEP_DEFAULT = 0.002; 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..e6456cba --- /dev/null +++ b/obelisk/cpp/obelisk_cpp/lidar/depth_interface.h @@ -0,0 +1,150 @@ +#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) + * 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) + * 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/cpp/obelisk_cpp/lidar/height_scan_interface.h b/obelisk/cpp/obelisk_cpp/lidar/height_scan_interface.h new file mode 100644 index 00000000..54856230 --- /dev/null +++ b/obelisk/cpp/obelisk_cpp/lidar/height_scan_interface.h @@ -0,0 +1,166 @@ +#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) + * 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) + * 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..428288ee --- /dev/null +++ b/obelisk/cpp/obelisk_cpp/lidar/lidar_interface.h @@ -0,0 +1,202 @@ +#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) + * 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) + * 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)); // 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; + + 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(); + + nv_ = nv; + nh_ = nh; + + // 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++; + } + } + + // 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 { + (void)hit_point; + return dist; + } + + 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 new file mode 100644 index 00000000..61e081b0 --- /dev/null +++ b/obelisk/cpp/obelisk_cpp/lidar/ray_caster_interface.h @@ -0,0 +1,254 @@ +#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; + } + + // Read frame (optional, default: "world") + 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; + + // 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 Get the frame_id string for published messages + */ + 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 + * @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; + + 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: + /** + * @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]; + std::string frame_ = "world"; + double max_distance_ = 0.0; // 0 => disabled +}; + +} // 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/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 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/depth_config.yaml b/obelisk_ws/src/obelisk_ros/config/depth_config.yaml new file mode 100644 index 00000000..e7fc4af3 --- /dev/null +++ b/obelisk_ws/src/obelisk_ros/config/depth_config.yaml @@ -0,0 +1,13 @@ +site_name: "pelvis_mocap_site" +geom_groups: [0, 1, 2] +viz_decimation: 1 +frame: "world" + +pattern: + 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.843391, 0., 0.537300, 0.] 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 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..45bd99f6 --- /dev/null +++ b/obelisk_ws/src/obelisk_ros/config/g1_scan_cpp.yaml @@ -0,0 +1,319 @@ +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: /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/depth_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 + # ---------- 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 + 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..d9cae7b8 --- /dev/null +++ b/obelisk_ws/src/obelisk_ros/config/height_scan_config.yaml @@ -0,0 +1,10 @@ +site_name: "pelvis_mocap_site" +geom_groups: [2] + +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..38558002 --- /dev/null +++ b/obelisk_ws/src/obelisk_ros/config/lidar_config.yaml @@ -0,0 +1,14 @@ +site_name: "head_lidar_site" +geom_groups: [2] +viz_decimation: 10 +frame: "body" + +pattern: + type: "lidar_scan" + vertical_fov_range: [0.0, 51.0] + horizontal_fov_range: [0., 360.] + channels: 51 + horizontal_res: 1.0 + offset: + 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 5f85bc14..63decc4d 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. --> + @@ -375,7 +376,7 @@ Position and velocity actuators added. --> - + @@ -792,12 +793,15 @@ Position and velocity actuators added. --> - - - - - - + + + + + + + + + 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 diff --git a/scripts/install_sys_deps.sh b/scripts/install_sys_deps.sh index e6fa4c7c..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-foxglove-bridge \ - ros-humble-joy # Need to make sure this is version 3.3.0 + ros-humble-joy + # NOTE: ros-humble-foxglove-bridge Dropped from humble for some reason. source /opt/ros/humble/setup.bash # python deps