diff --git a/controllers/ocs2_quadruped_controller/CMakeLists.txt b/controllers/ocs2_quadruped_controller/CMakeLists.txt index c22562c..39c656b 100644 --- a/controllers/ocs2_quadruped_controller/CMakeLists.txt +++ b/controllers/ocs2_quadruped_controller/CMakeLists.txt @@ -63,13 +63,16 @@ add_library(${PROJECT_NAME} SHARED src/control/TargetManager.cpp src/control/CtrlComponent.cpp - src/perceptive_interface/constraint/FootCollisionConstraint.cpp - src/perceptive_interface/constraint/FootPlacementConstraint.cpp - src/perceptive_interface/constraint/SphereSdfConstraint.cpp - src/perceptive_interface/ConvexRegionSelector.cpp - src/perceptive_interface/PerceptiveLeggedInterface.cpp - src/perceptive_interface/PerceptiveLeggedPrecomputation.cpp - src/perceptive_interface/PerceptiveLeggedReferenceManager.cpp + src/perceptive/constraint/FootCollisionConstraint.cpp + src/perceptive/constraint/FootPlacementConstraint.cpp + src/perceptive/constraint/SphereSdfConstraint.cpp + src/perceptive/interface/ConvexRegionSelector.cpp + src/perceptive/interface/PerceptiveLeggedInterface.cpp + src/perceptive/interface/PerceptiveLeggedPrecomputation.cpp + src/perceptive/interface/PerceptiveLeggedReferenceManager.cpp + src/perceptive/visualize/FootPlacementVisualization.cpp + src/perceptive/visualize/SphereVisualization.cpp + src/perceptive/synchronize/PlanarTerrainReceiver.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC diff --git a/controllers/ocs2_quadruped_controller/config/convex_plane_decomposition.yaml b/controllers/ocs2_quadruped_controller/config/convex_plane_decomposition.yaml new file mode 100644 index 0000000..2c596a9 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/config/convex_plane_decomposition.yaml @@ -0,0 +1,34 @@ +preprocessing: + resolution: 0.03 # Resampling resolution, set negative to skip, requires inpainting to be used + kernelSize: 3 # Kernel size of the median filter, either 3 or 5 + numberOfRepeats: 1 # Number of times to apply the same filter + +sliding_window_plane_extractor: + kernel_size: 3 # Size of the sliding window patch used for normal vector calculation and planarity detection. Should be an odd number and at least 3. + planarity_opening_filter: 0 # [#] Apply opening filter (erosion -> dilation) to planarity detection by this amount of pixels + plane_inclination_threshold_degrees: 30.0 # [deg] Maximum allowed angle between the surface normal and the world-z direction for a patch + local_plane_inclination_threshold_degrees: 35.0 # [deg] Maximum allowed angle between the surface normal and the world-z direction for a cell + plane_patch_error_threshold: 0.02 # [m] The allowed root-mean-squared deviation from the plane fitted to the sliding window. + min_number_points_per_label: 4 # [#] Labels with less points assigned to them are discarded + connectivity: 4 # Label kernel connectivity. 4 or 8 (cross or box) + include_ransac_refinement: true # Enable RANSAC refinement if the plane is not globally fitting to the assigned points. + global_plane_fit_distance_error_threshold: 0.025 # [m] Allowed maximum distance from a point to the plane. If any point violates this, RANSAC is triggered + global_plane_fit_angle_error_threshold_degrees: 25.0 # [deg] Allowed normal vector deviation for a point w.r.t. the plane normal. If any point violates this, RANSAC is triggered + +ransac_plane_refinement: + probability: 0.001 # Probability to miss the largest candidate shape. A lower probability provides a higher reliability and determinism at the cost of longer running times + min_points: 4 # This minimum number of points per plane. Controls the termination of the algorithm. + epsilon: 0.025 # Maximum distance to plane + cluster_epsilon: 0.041 # [m] Set maximum Euclidean distance between points to be clustered. Two points are connected if separated by a distance of at most 2*sqrt(2)*cluster_epsilon = 2.828 * cluster_epsilon. Set this higher than resolution + normal_threshold: 25.0 # [deg] Set maximum normal deviation between cluster surface_normal and point normal. + +contour_extraction: + marginSize: 1 # Size of the kernel creating the boundary offset. In number of (sub) pixels. + +postprocessing: + extracted_planes_height_offset: 0.02 # Added offset in Z direction for the full map to compensate for the location of the foot frame w.r.t. the contact point + nonplanar_height_offset: 0.03 # Added offset in Z direction for non-planar cells of the map. (Additional to extracted_planes_height_offset) + nonplanar_horizontal_offset: 3 # Added offset in XY direction for non-planar cells of the map. In number of pixels. + smoothing_dilation_size: 0.2 # Half the width of the dilation used before the smooth layer [m] + smoothing_box_kernel_size: 0.1 # Half the width of the box kernel used for the smooth layer [m] + smoothing_gauss_kernel_size: 0.05 # Half the width of the Gaussian kernel used for the smooth layer [m] diff --git a/controllers/ocs2_quadruped_controller/config/elevation_mapping.yaml b/controllers/ocs2_quadruped_controller/config/elevation_mapping.yaml new file mode 100644 index 0000000..a25fbaf --- /dev/null +++ b/controllers/ocs2_quadruped_controller/config/elevation_mapping.yaml @@ -0,0 +1,48 @@ +# Robot. +elevation_mapping: + ros__parameters: + map_frame_id: odom + robot_base_frame_id: base + robot_pose_with_covariance_topic: /pose + input_sources: + front: # A name to identify the input source + type: pointcloud # Supported types: pointcloud + topic: /rgbd_d435/points + queue_size: 1 + publish_on_update: true + sensor_processor: + type: perfect + lidar: + type: pointcloud + topic: /scan/points + queue_size: 1 + publish_on_update: true + sensor_processor: + type: laser + ignore_points_above: 0.5 + ignore_points_below: -0.5 + + track_point_frame_id: base + track_point_x: 0.0 + track_point_y: 0.0 + track_point_z: 0.0 + time_tolerance: 1e-3 + scanning_duration: 0.1 + visibility_cleanup_rate: 5 + + # Map. + length_in_x: 5.0 + length_in_y: 5.0 + position_x: 0.0 + position_y: 0.0 + resolution: 0.03 + min_variance: 0.0001 + max_variance: 0.05 + mahalanobis_distance_threshold: 2.5 + multi_height_noise: 0.001 + surface_normal_positive_axis: z + initialize_elevation_map: true + length_in_x_init_submap: 3.0 + length_in_y_init_submap: 3.0 + init_submap_variance: 1e-3 + target_frame_init_submap: odom diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h index a51da52..2cec804 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h @@ -13,6 +13,8 @@ #include #include #include +#include +#include #include "TargetManager.h" @@ -60,11 +62,15 @@ namespace ocs2::legged_robot void setupMpc(); void setupMrt(); + bool enable_perceptive_ = false; CtrlInterfaces& ctrl_interfaces_; std::unique_ptr estimator_; std::unique_ptr rbd_conversions_; std::unique_ptr target_manager_; + std::unique_ptr footPlacementVisualizationPtr_; + std::unique_ptr sphereVisualizationPtr_; + std::vector joint_names_; std::vector feet_names_; std::string robot_pkg_; diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h index 4dbb5b6..c6956b8 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h @@ -10,6 +10,8 @@ #include #include #include +#include +#include namespace ocs2::legged_robot { @@ -17,6 +19,7 @@ namespace ocs2::legged_robot { public: TargetManager(CtrlInterfaces& ctrl_component, + rclcpp_lifecycle::LifecycleNode::SharedPtr node, const std::shared_ptr& referenceManagerPtr, const std::string& task_file, const std::string& reference_file); @@ -51,11 +54,16 @@ namespace ocs2::legged_robot CtrlInterfaces& ctrl_component_; std::shared_ptr referenceManagerPtr_; + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + rclcpp::Subscription::SharedPtr twist_sub_; + realtime_tools::RealtimeBuffer buffer_; + int twist_count = 0; + vector_t default_joint_state_{}; scalar_t command_height_{}; scalar_t time_to_target_{}; - scalar_t target_displacement_velocity_; - scalar_t target_rotation_velocity_; + scalar_t target_displacement_velocity_{}; + scalar_t target_rotation_velocity_{}; }; } diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/FootCollisionConstraint.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/constraint/FootCollisionConstraint.h similarity index 98% rename from controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/FootCollisionConstraint.h rename to controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/constraint/FootCollisionConstraint.h index 520e67a..fc78770 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/FootCollisionConstraint.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/constraint/FootCollisionConstraint.h @@ -1,5 +1,5 @@ // -// Created by qiayuan on 23-1-1. +// Created by biao on 3/21/25. // #pragma once diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/FootPlacementConstraint.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/constraint/FootPlacementConstraint.h similarity index 98% rename from controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/FootPlacementConstraint.h rename to controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/constraint/FootPlacementConstraint.h index dc213dc..e647c45 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/FootPlacementConstraint.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/constraint/FootPlacementConstraint.h @@ -1,5 +1,5 @@ // -// Created by qiayuan on 23-1-1. +// Created by biao on 3/21/25. // #pragma once diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/SphereSdfConstraint.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/constraint/SphereSdfConstraint.h similarity index 97% rename from controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/SphereSdfConstraint.h rename to controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/constraint/SphereSdfConstraint.h index e5448ae..ca6cf03 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/SphereSdfConstraint.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/constraint/SphereSdfConstraint.h @@ -1,5 +1,5 @@ // -// Created by qiayuan on 22-12-27. +// Created by biao on 3/21/25. // #pragma once diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/interface/ConvexRegionSelector.h similarity index 98% rename from controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h rename to controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/interface/ConvexRegionSelector.h index 933b131..20d087e 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/interface/ConvexRegionSelector.h @@ -1,5 +1,5 @@ // -// Created by qiayuan on 23-1-2. +// Created by biao on 3/21/25. // #pragma once diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedInterface.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedInterface.h similarity index 98% rename from controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedInterface.h rename to controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedInterface.h index d6ca037..5f182b8 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedInterface.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedInterface.h @@ -1,5 +1,5 @@ // -// Created by qiayuan on 22-12-27. +// Created by biao on 3/21/25. // #pragma once diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedPrecomputation.h similarity index 89% rename from controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h rename to controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedPrecomputation.h index 97cbe49..77b54ee 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedPrecomputation.h @@ -1,5 +1,5 @@ // -// Created by qiayuan on 23-1-1. +// Created by biao on 3/21/25. // #pragma once @@ -9,8 +9,8 @@ #include #include -#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h" -#include "ocs2_quadruped_controller/perceptive_interface/constraint/FootPlacementConstraint.h" +#include "ocs2_quadruped_controller/perceptive/interface/ConvexRegionSelector.h" +#include "ocs2_quadruped_controller/perceptive/constraint/FootPlacementConstraint.h" namespace ocs2::legged_robot { diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedReferenceManager.h similarity index 96% rename from controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h rename to controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedReferenceManager.h index 41f6e95..83d5304 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedReferenceManager.h @@ -1,11 +1,12 @@ // -// Created by qiayuan on 23-1-3. +// Created by biao on 3/21/25. // + #pragma once #include -#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h" +#include "ocs2_quadruped_controller/perceptive/interface/ConvexRegionSelector.h" #include diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/synchronize/PlanarTerrainReceiver.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/synchronize/PlanarTerrainReceiver.h new file mode 100644 index 0000000..5913e8d --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/synchronize/PlanarTerrainReceiver.h @@ -0,0 +1,53 @@ +// +// Created by biao on 3/21/25. +// + +#ifndef PLANARTERRAINRECEIVER_H +#define PLANARTERRAINRECEIVER_H + +#include +#include +#include + +#include + +#include +#include +#include + +namespace ocs2::legged_robot +{ + class PlanarTerrainReceiver : public SolverSynchronizedModule + { + public: + PlanarTerrainReceiver(const rclcpp_lifecycle::LifecycleNode::SharedPtr& node, + const std::shared_ptr& planarTerrainPtr, + const std::shared_ptr& signedDistanceFieldPtr, + const std::string& mapTopic, + const std::string& sdfElevationLayer); + + void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, + const ReferenceManagerInterface& referenceManager) override; + + void postSolverRun(const PrimalSolution& primalSolution) override + { + } + + private: + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + rclcpp::Subscription::SharedPtr subscription_; + + convex_plane_decomposition::PlanarTerrain planarTerrain_; + + std::string sdfElevationLayer_; + + std::mutex mutex_; + std::atomic_bool updated_ = false; + + std::shared_ptr planarTerrainPtr_; + std::shared_ptr sdfPtr_; + }; +} + + +#endif //PLANARTERRAINRECEIVER_H diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/visualize/FootPlacementVisualization.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/visualize/FootPlacementVisualization.h new file mode 100644 index 0000000..337e47a --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/visualize/FootPlacementVisualization.h @@ -0,0 +1,45 @@ +// +// Created by biao on 3/21/25. +// + +#pragma once + +#include +#include +#include "ocs2_quadruped_controller/perceptive/interface/ConvexRegionSelector.h" +#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedPrecomputation.h" + +#include +#include +#include +#include + +namespace ocs2::legged_robot +{ + class FootPlacementVisualization + { + public: + FootPlacementVisualization(const ConvexRegionSelector& convexRegionSelector, size_t numFoot, + const rclcpp_lifecycle::LifecycleNode::SharedPtr& node, + scalar_t maxUpdateFrequency = 20.0); + + void update(const SystemObservation& observation); + + private: + visualization_msgs::msg::Marker to3dRosMarker(const convex_plane_decomposition::CgalPolygon2d& polygon, + const Eigen::Isometry3d& transformPlaneToWorld, + const std_msgs::msg::Header& header, Color color, + float alpha, size_t i) const; + + scalar_t line_width_ = 0.008; + scalar_t foot_marker_diameter_ = 0.02; + std::vector feet_color_map_ = {Color::blue, Color::orange, Color::yellow, Color::purple}; + + const ConvexRegionSelector& convex_region_selector_; + + size_t num_foot_; + rclcpp::Publisher::SharedPtr marker_publisher_; + scalar_t last_time_; + scalar_t min_publish_time_difference_; + }; +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/visualize/SphereVisualization.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/visualize/SphereVisualization.h new file mode 100644 index 0000000..357197d --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive/visualize/SphereVisualization.h @@ -0,0 +1,35 @@ +// +// Created by biao on 3/21/25. +// + +#pragma once +#include +#include + +#include +#include +#include +#include +#include + +namespace ocs2::legged_robot +{ + class SphereVisualization + { + public: + SphereVisualization(PinocchioInterface pinocchioInterface, CentroidalModelInfo centroidalModelInfo, + const PinocchioSphereInterface& sphereInterface, + const rclcpp_lifecycle::LifecycleNode::SharedPtr& node, + scalar_t maxUpdateFrequency = 100.0); + void update(const SystemObservation& observation); + + private: + PinocchioInterface pinocchio_interface_; + const CentroidalModelInfo centroidal_model_info_; + const PinocchioSphereInterface& sphere_interface_; + rclcpp::Publisher::SharedPtr marker_publisher_; + + scalar_t last_time_; + scalar_t min_publish_time_difference_; + }; +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/launch/elevation_mapping.launch.py b/controllers/ocs2_quadruped_controller/launch/elevation_mapping.launch.py new file mode 100644 index 0000000..e69de29 diff --git a/controllers/ocs2_quadruped_controller/src/control/CtrlComponent.cpp b/controllers/ocs2_quadruped_controller/src/control/CtrlComponent.cpp index 0614c08..1298b8e 100644 --- a/controllers/ocs2_quadruped_controller/src/control/CtrlComponent.cpp +++ b/controllers/ocs2_quadruped_controller/src/control/CtrlComponent.cpp @@ -16,6 +16,9 @@ #include #include #include +#include +#include +#include #include namespace ocs2::legged_robot @@ -25,10 +28,12 @@ namespace ocs2::legged_robot { node_->declare_parameter("robot_pkg", robot_pkg_); node_->declare_parameter("feet", feet_names_); + node_->declare_parameter("enable_perceptive", enable_perceptive_); robot_pkg_ = node_->get_parameter("robot_pkg").as_string(); joint_names_ = node_->get_parameter("joints").as_string_array(); feet_names_ = node_->get_parameter("feet").as_string_array(); + enable_perceptive_ = node_->get_parameter("enable_perceptive").as_bool(); const std::string package_share_directory = ament_index_cpp::get_package_share_directory(robot_pkg_); @@ -105,6 +110,11 @@ namespace ocs2::legged_robot observation_.mode = estimator_->getMode(); visualizer_->update(observation_); + if (enable_perceptive_) + { + footPlacementVisualizationPtr_->update(observation_); + sphereVisualizationPtr_->update(observation_); + } // Compute target trajectory target_manager_->update(observation_); @@ -137,9 +147,29 @@ namespace ocs2::legged_robot void CtrlComponent::setupLeggedInterface() { - legged_interface_ = std::make_unique(task_file_, urdf_file_, reference_file_); + if (enable_perceptive_) + { + legged_interface_ = std::make_unique(task_file_, urdf_file_, reference_file_); + } + else + { + legged_interface_ = std::make_unique(task_file_, urdf_file_, reference_file_); + } + legged_interface_->setupJointNames(joint_names_, feet_names_); legged_interface_->setupOptimalControlProblem(task_file_, urdf_file_, reference_file_, verbose_); + + if (enable_perceptive_) + { + footPlacementVisualizationPtr_ = std::make_unique( + *dynamic_cast(*legged_interface_->getReferenceManagerPtr()). + getConvexRegionSelectorPtr(), + legged_interface_->getCentroidalModelInfo().numThreeDofContacts, node_); + + sphereVisualizationPtr_ = std::make_unique( + legged_interface_->getPinocchioInterface(), legged_interface_->getCentroidalModelInfo(), + *dynamic_cast(*legged_interface_).getPinocchioSphereInterfacePtr(), node_); + } } /** @@ -162,9 +192,20 @@ namespace ocs2::legged_robot mpc_->getSolverPtr()->setReferenceManager(legged_interface_->getReferenceManagerPtr()); target_manager_ = std::make_unique(ctrl_interfaces_, + node_, legged_interface_->getReferenceManagerPtr(), task_file_, reference_file_); + + if (enable_perceptive_) + { + const auto planarTerrainReceiver = + std::make_shared( + node_, dynamic_cast(*legged_interface_).getPlanarTerrainPtr(), + dynamic_cast(*legged_interface_).getSignedDistanceFieldPtr(), + "/convex_plane_decomposition_ros/planar_terrain", "elevation"); + mpc_->getSolverPtr()->addSynchronizedModule(planarTerrainReceiver); + } } void CtrlComponent::setupMrt() diff --git a/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp b/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp index 974eca9..13079d7 100644 --- a/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp +++ b/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp @@ -7,14 +7,18 @@ #include #include +#include + namespace ocs2::legged_robot { TargetManager::TargetManager(CtrlInterfaces& ctrl_component, + rclcpp_lifecycle::LifecycleNode::SharedPtr node, const std::shared_ptr& referenceManagerPtr, const std::string& task_file, const std::string& reference_file) : ctrl_component_(ctrl_component), - referenceManagerPtr_(referenceManagerPtr) + referenceManagerPtr_(referenceManagerPtr), + node_(std::move(node)) { default_joint_state_ = vector_t::Zero(12); loadData::loadCppDataType(reference_file, "comHeight", command_height_); @@ -22,21 +26,45 @@ namespace ocs2::legged_robot loadData::loadCppDataType(task_file, "mpc.timeHorizon", time_to_target_); loadData::loadCppDataType(reference_file, "targetRotationVelocity", target_rotation_velocity_); loadData::loadCppDataType(reference_file, "targetDisplacementVelocity", target_displacement_velocity_); + + twist_sub_ = node_->create_subscription( + "/cmd_vel", 10, [this](const geometry_msgs::msg::Twist::SharedPtr msg) + { + buffer_.writeFromNonRT(*msg); + twist_count = ctrl_component_.frequency_ / 5; + RCLCPP_INFO(node_->get_logger(), "Twist count: %i", twist_count); + }); } void TargetManager::update(SystemObservation& observation) { vector_t cmdGoal = vector_t::Zero(6); - cmdGoal[0] = ctrl_component_.control_inputs_.ly * target_displacement_velocity_; - cmdGoal[1] = -ctrl_component_.control_inputs_.lx * target_displacement_velocity_; - cmdGoal[2] = ctrl_component_.control_inputs_.ry; - cmdGoal[3] = -ctrl_component_.control_inputs_.rx * target_rotation_velocity_; + if (buffer_.readFromRT() == nullptr) + { + cmdGoal[0] = ctrl_component_.control_inputs_.ly * target_displacement_velocity_; + cmdGoal[1] = -ctrl_component_.control_inputs_.lx * target_displacement_velocity_; + cmdGoal[2] = ctrl_component_.control_inputs_.ry; + cmdGoal[3] = -ctrl_component_.control_inputs_.rx * target_rotation_velocity_; + } + else + { + const geometry_msgs::msg::Twist twist = *buffer_.readFromRT(); + cmdGoal[0] = twist.linear.x; + cmdGoal[1] = twist.linear.y; + cmdGoal[2] = 0; + cmdGoal[3] = twist.angular.z; + twist_count--; + if (twist_count <= 0) + { + buffer_.reset(); + } + } const vector_t currentPose = observation.state.segment<6>(6); const Eigen::Matrix zyx = currentPose.tail(3); vector_t cmd_vel_rot = getRotationMatrixFromZyxEulerAngles(zyx) * cmdGoal.head(3); - const vector_t targetPose = [&]() + const vector_t targetPose = [&] { vector_t target(6); target(0) = currentPose(0) + cmd_vel_rot(0) * time_to_target_; diff --git a/controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/FootCollisionConstraint.cpp b/controllers/ocs2_quadruped_controller/src/perceptive/constraint/FootCollisionConstraint.cpp similarity index 95% rename from controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/FootCollisionConstraint.cpp rename to controllers/ocs2_quadruped_controller/src/perceptive/constraint/FootCollisionConstraint.cpp index 846a2a7..abbbb89 100644 --- a/controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/FootCollisionConstraint.cpp +++ b/controllers/ocs2_quadruped_controller/src/perceptive/constraint/FootCollisionConstraint.cpp @@ -1,7 +1,7 @@ // -// Created by qiayuan on 23-1-26. +// Created by biao on 3/21/25. // -#include "ocs2_quadruped_controller/perceptive_interface/constraint/FootCollisionConstraint.h" +#include "ocs2_quadruped_controller/perceptive/constraint/FootCollisionConstraint.h" namespace ocs2::legged_robot { diff --git a/controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/FootPlacementConstraint.cpp b/controllers/ocs2_quadruped_controller/src/perceptive/constraint/FootPlacementConstraint.cpp similarity index 90% rename from controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/FootPlacementConstraint.cpp rename to controllers/ocs2_quadruped_controller/src/perceptive/constraint/FootPlacementConstraint.cpp index 583986f..16c7032 100644 --- a/controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/FootPlacementConstraint.cpp +++ b/controllers/ocs2_quadruped_controller/src/perceptive/constraint/FootPlacementConstraint.cpp @@ -1,10 +1,10 @@ // -// Created by qiayuan on 23-1-1. +// Created by biao on 3/21/25. // -#include "ocs2_quadruped_controller/perceptive_interface/constraint/FootPlacementConstraint.h" -#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h" -#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h" +#include "ocs2_quadruped_controller/perceptive/constraint/FootPlacementConstraint.h" +#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedPrecomputation.h" +#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedReferenceManager.h" namespace ocs2::legged_robot { diff --git a/controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/SphereSdfConstraint.cpp b/controllers/ocs2_quadruped_controller/src/perceptive/constraint/SphereSdfConstraint.cpp similarity index 92% rename from controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/SphereSdfConstraint.cpp rename to controllers/ocs2_quadruped_controller/src/perceptive/constraint/SphereSdfConstraint.cpp index 1a109d3..807aefc 100644 --- a/controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/SphereSdfConstraint.cpp +++ b/controllers/ocs2_quadruped_controller/src/perceptive/constraint/SphereSdfConstraint.cpp @@ -1,9 +1,9 @@ // -// Created by qiayuan on 22-12-27. +// Created by biao on 3/21/25. // -#include "ocs2_quadruped_controller/perceptive_interface/constraint/SphereSdfConstraint.h" -#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h" +#include "ocs2_quadruped_controller/perceptive/constraint/SphereSdfConstraint.h" +#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedPrecomputation.h" namespace ocs2::legged_robot { diff --git a/controllers/ocs2_quadruped_controller/src/perceptive_interface/ConvexRegionSelector.cpp b/controllers/ocs2_quadruped_controller/src/perceptive/interface/ConvexRegionSelector.cpp similarity index 98% rename from controllers/ocs2_quadruped_controller/src/perceptive_interface/ConvexRegionSelector.cpp rename to controllers/ocs2_quadruped_controller/src/perceptive/interface/ConvexRegionSelector.cpp index 90bd0b2..ac847f3 100644 --- a/controllers/ocs2_quadruped_controller/src/perceptive_interface/ConvexRegionSelector.cpp +++ b/controllers/ocs2_quadruped_controller/src/perceptive/interface/ConvexRegionSelector.cpp @@ -1,7 +1,8 @@ // -// Created by qiayuan on 23-1-2. +// Created by biao on 3/21/25. // -#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h" + +#include #include #include diff --git a/controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedInterface.cpp b/controllers/ocs2_quadruped_controller/src/perceptive/interface/PerceptiveLeggedInterface.cpp similarity index 93% rename from controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedInterface.cpp rename to controllers/ocs2_quadruped_controller/src/perceptive/interface/PerceptiveLeggedInterface.cpp index 224d8ef..a9ee2a2 100644 --- a/controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedInterface.cpp +++ b/controllers/ocs2_quadruped_controller/src/perceptive/interface/PerceptiveLeggedInterface.cpp @@ -1,15 +1,15 @@ // -// Created by qiayuan on 22-12-27. +// Created by biao on 3/21/25. // -#include "ocs2_quadruped_controller/perceptive_interface/constraint/FootCollisionConstraint.h" -#include "ocs2_quadruped_controller/perceptive_interface/constraint/SphereSdfConstraint.h" #include +#include "ocs2_quadruped_controller/perceptive/constraint/FootCollisionConstraint.h" +#include "ocs2_quadruped_controller/perceptive/constraint/SphereSdfConstraint.h" -#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h" -#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedInterface.h" -#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h" -#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h" +#include "ocs2_quadruped_controller/perceptive/interface/ConvexRegionSelector.h" +#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedInterface.h" +#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedPrecomputation.h" +#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedReferenceManager.h" #include #include diff --git a/controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedPrecomputation.cpp b/controllers/ocs2_quadruped_controller/src/perceptive/interface/PerceptiveLeggedPrecomputation.cpp similarity index 97% rename from controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedPrecomputation.cpp rename to controllers/ocs2_quadruped_controller/src/perceptive/interface/PerceptiveLeggedPrecomputation.cpp index ad05720..8e6c520 100644 --- a/controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedPrecomputation.cpp +++ b/controllers/ocs2_quadruped_controller/src/perceptive/interface/PerceptiveLeggedPrecomputation.cpp @@ -1,8 +1,8 @@ // -// Created by qiayuan on 23-1-1. +// Created by biao on 3/21/25. // -#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h" +#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedPrecomputation.h" namespace ocs2::legged_robot { diff --git a/controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedReferenceManager.cpp b/controllers/ocs2_quadruped_controller/src/perceptive/interface/PerceptiveLeggedReferenceManager.cpp similarity index 98% rename from controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedReferenceManager.cpp rename to controllers/ocs2_quadruped_controller/src/perceptive/interface/PerceptiveLeggedReferenceManager.cpp index b7cd72c..78a26a0 100644 --- a/controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedReferenceManager.cpp +++ b/controllers/ocs2_quadruped_controller/src/perceptive/interface/PerceptiveLeggedReferenceManager.cpp @@ -1,11 +1,11 @@ // -// Created by qiayuan on 23-1-3. +// Created by biao on 3/21/25. // #include #include #include -#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h" +#include "ocs2_quadruped_controller/perceptive/interface/PerceptiveLeggedReferenceManager.h" namespace ocs2::legged_robot { diff --git a/controllers/ocs2_quadruped_controller/src/perceptive/synchronize/PlanarTerrainReceiver.cpp b/controllers/ocs2_quadruped_controller/src/perceptive/synchronize/PlanarTerrainReceiver.cpp new file mode 100644 index 0000000..8eaa6fc --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/perceptive/synchronize/PlanarTerrainReceiver.cpp @@ -0,0 +1,59 @@ +// +// Created by biao on 3/21/25. +// + +#include "ocs2_quadruped_controller/perceptive/synchronize/PlanarTerrainReceiver.h" + +#include +#include +#include + +namespace ocs2::legged_robot +{ + PlanarTerrainReceiver::PlanarTerrainReceiver(const rclcpp_lifecycle::LifecycleNode::SharedPtr& node, + const std::shared_ptr + & planarTerrainPtr, + const std::shared_ptr& + signedDistanceFieldPtr, + const std::string& mapTopic, const std::string& sdfElevationLayer) + : node_(node), + planarTerrainPtr_(planarTerrainPtr), + sdfPtr_(signedDistanceFieldPtr), + sdfElevationLayer_(sdfElevationLayer) + { + subscription_ = node_->create_subscription( + mapTopic, 10, [this](const convex_plane_decomposition_msgs::msg::PlanarTerrain msg) + { + std::lock_guard lock(mutex_); + updated_ = true; + + planarTerrain_ = convex_plane_decomposition::PlanarTerrain( + convex_plane_decomposition::fromMessage(msg)); + + auto& elevationData = planarTerrain_.gridMap.get(sdfElevationLayer_); + if (elevationData.hasNaN()) + { + const float inpaint{elevationData.minCoeffOfFinites()}; + RCLCPP_WARN(node_->get_logger(), + "[PlanarTerrainReceiver] Map contains NaN values. Will apply inpainting with min value.") + ; + elevationData = elevationData.unaryExpr([=](float v) { return std::isfinite(v) ? v : inpaint; }); + } + constexpr float heightMargin{0.1}; + const float maxValue{elevationData.maxCoeffOfFinites() + 3 * heightMargin}; + sdfPtr_->calculateSignedDistanceField(planarTerrain_.gridMap, sdfElevationLayer_, maxValue); + }); + } + + void PlanarTerrainReceiver::preSolverRun(scalar_t /*initTime*/, scalar_t /*finalTime*/, + const vector_t& /*currentState*/, + const ReferenceManagerInterface& /*referenceManager*/) + { + if (updated_) + { + std::lock_guard lock(mutex_); + updated_ = false; + *planarTerrainPtr_ = planarTerrain_; + } + } +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/perceptive/visualize/FootPlacementVisualization.cpp b/controllers/ocs2_quadruped_controller/src/perceptive/visualize/FootPlacementVisualization.cpp new file mode 100644 index 0000000..56d68b1 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/perceptive/visualize/FootPlacementVisualization.cpp @@ -0,0 +1,130 @@ +// +// Created by biao on 3/21/25. +// + +#include "ocs2_quadruped_controller/perceptive/visualize/FootPlacementVisualization.h" +#include +#include +#include + +namespace ocs2::legged_robot +{ + FootPlacementVisualization::FootPlacementVisualization(const ConvexRegionSelector& convexRegionSelector, + size_t numFoot, + const rclcpp_lifecycle::LifecycleNode::SharedPtr& node, + scalar_t maxUpdateFrequency) + : convex_region_selector_(convexRegionSelector), + num_foot_(numFoot), + last_time_(std::numeric_limits::lowest()), + min_publish_time_difference_(1.0 / maxUpdateFrequency) + { + marker_publisher_ = node->create_publisher("foot_placement", 1); + } + + void FootPlacementVisualization::update(const SystemObservation& observation) + { + if (observation.time - last_time_ > min_publish_time_difference_) + { + last_time_ = observation.time; + + std_msgs::msg::Header header; + // header.stamp.fromNSec(planarTerrainPtr->gridMap.getTimestamp()); + header.frame_id = "odom"; + + visualization_msgs::msg::MarkerArray makerArray; + + size_t i = 0; + for (int leg = 0; leg < num_foot_; ++leg) + { + auto middleTimes = convex_region_selector_.getMiddleTimes(leg); + + int kStart = 0; + for (int k = 0; k < middleTimes.size(); ++k) + { + const auto projection = convex_region_selector_.getProjection(leg, middleTimes[k]); + if (projection.regionPtr == nullptr) + { + continue; + } + if (middleTimes[k] < observation.time) + { + kStart = k + 1; + continue; + } + auto color = feet_color_map_[leg]; + float alpha = 1 - static_cast(k - kStart) / static_cast(middleTimes.size() - kStart); + // Projections + auto projectionMaker = getArrowAtPointMsg( + projection.regionPtr->transformPlaneToWorld.linear() * vector3_t(0, 0, 0.1), + projection.positionInWorld, color); + projectionMaker.header = header; + projectionMaker.ns = "Projections"; + projectionMaker.id = i; + projectionMaker.color.a = alpha; + makerArray.markers.push_back(projectionMaker); + + // Convex Region + const auto convexRegion = convex_region_selector_.getConvexPolygon(leg, middleTimes[k]); + auto convexRegionMsg = + convex_plane_decomposition::to3dRosPolygon(convexRegion, + projection.regionPtr->transformPlaneToWorld, header); + makerArray.markers.push_back(to3dRosMarker(convexRegion, + projection.regionPtr->transformPlaneToWorld, header, + color, alpha, i)); + + // Nominal Footholds + const auto nominal = convex_region_selector_.getNominalFootholds(leg, middleTimes[k]); + auto nominalMarker = getFootMarker(nominal, true, color, foot_marker_diameter_, 1.); + nominalMarker.header = header; + nominalMarker.ns = "Nominal Footholds"; + nominalMarker.id = i; + nominalMarker.color.a = alpha; + makerArray.markers.push_back(nominalMarker); + + i++; + } + } + + marker_publisher_->publish(makerArray); + } + } + + visualization_msgs::msg::Marker FootPlacementVisualization::to3dRosMarker( + const convex_plane_decomposition::CgalPolygon2d& polygon, + const Eigen::Isometry3d& transformPlaneToWorld, + const std_msgs::msg::Header& header, Color color, float alpha, size_t i) const + { + visualization_msgs::msg::Marker marker; + marker.ns = "Convex Regions"; + marker.id = i; + marker.header = header; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.scale.x = line_width_; + marker.color = getColor(color, alpha); + if (!polygon.is_empty()) + { + marker.points.reserve(polygon.size() + 1); + for (const auto& point : polygon) + { + const auto pointInWorld = convex_plane_decomposition::positionInWorldFrameFromPosition2dInPlane( + point, transformPlaneToWorld); + geometry_msgs::msg::Point point_ros; + point_ros.x = pointInWorld.x(); + point_ros.y = pointInWorld.y(); + point_ros.z = pointInWorld.z(); + marker.points.push_back(point_ros); + } + // repeat the first point to close to polygon + const auto pointInWorld = + convex_plane_decomposition::positionInWorldFrameFromPosition2dInPlane( + polygon.vertex(0), transformPlaneToWorld); + geometry_msgs::msg::Point point_ros; + point_ros.x = pointInWorld.x(); + point_ros.y = pointInWorld.y(); + point_ros.z = pointInWorld.z(); + marker.points.push_back(point_ros); + } + marker.pose.orientation.w = 1.0; + return marker; + } +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/perceptive/visualize/SphereVisualization.cpp b/controllers/ocs2_quadruped_controller/src/perceptive/visualize/SphereVisualization.cpp new file mode 100644 index 0000000..df66597 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/perceptive/visualize/SphereVisualization.cpp @@ -0,0 +1,73 @@ +// +// Created by biao on 3/21/25. +// + +#include + +#include +#include + +#include "ocs2_quadruped_controller/perceptive/visualize/SphereVisualization.h" + +#include +#include +#include + +namespace ocs2::legged_robot +{ + SphereVisualization::SphereVisualization(PinocchioInterface pinocchioInterface, + CentroidalModelInfo centroidalModelInfo, + const PinocchioSphereInterface& sphereInterface, + const rclcpp_lifecycle::LifecycleNode::SharedPtr& node, + const scalar_t maxUpdateFrequency) + : pinocchio_interface_(std::move(pinocchioInterface)), + centroidal_model_info_(std::move(centroidalModelInfo)), + sphere_interface_(sphereInterface), + last_time_(std::numeric_limits::lowest()), + min_publish_time_difference_(1.0 / maxUpdateFrequency) + { + marker_publisher_ = node->create_publisher("sphere_markers", 1); + } + + void SphereVisualization::update(const SystemObservation& observation) + { + if (observation.time - last_time_ > min_publish_time_difference_) + { + last_time_ = observation.time; + + visualization_msgs::msg::MarkerArray markers; + + const auto& model = pinocchio_interface_.getModel(); + auto& data = pinocchio_interface_.getData(); + forwardKinematics(model, data, + centroidal_model::getGeneralizedCoordinates( + observation.state, centroidal_model_info_)); + + auto positions = sphere_interface_.computeSphereCentersInWorldFrame(pinocchio_interface_); + auto numSpheres = sphere_interface_.getNumSpheres(); + auto rads = sphere_interface_.getSphereRadii(); + + int k = 0; + for (int i = 0; i < numSpheres.size(); ++i) + { + visualization_msgs::msg::Marker marker; + marker.id = i; + marker.header.frame_id = "odom"; + marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; + marker.color = getColor(Color::red, 0.5); + marker.pose.orientation = ros_msg_helpers::getOrientationMsg({1, 0, 0, 0}); + marker.scale.x = rads[k]; + marker.scale.y = marker.scale.x; + marker.scale.z = marker.scale.x; + for (int j = 0; j < numSpheres[i]; ++j) + { + marker.points.push_back(ros_msg_helpers::getPointMsg(positions[k + j])); + } + k += numSpheres[i]; + + markers.markers.push_back(marker); + } + marker_publisher_->publish(markers); + } + } +} // namespace legged diff --git a/descriptions/deep_robotics/lite3_description/config/ocs2/task.info b/descriptions/deep_robotics/lite3_description/config/ocs2/task.info index 6664796..ed6c97c 100644 --- a/descriptions/deep_robotics/lite3_description/config/ocs2/task.info +++ b/descriptions/deep_robotics/lite3_description/config/ocs2/task.info @@ -12,7 +12,7 @@ model_settings verboseCppAd true recompileLibrariesCppAd false - modelFolderCppAd /tmp/ocs2_quadruped_controller/lite3 + modelFolderCppAd ocs2_cpp_ad/lite3 } swing_trajectory_config diff --git a/libraries/gz_quadruped_playground/launch/ocs2.launch.py b/libraries/gz_quadruped_playground/launch/ocs2.launch.py index ffd591e..6cdd75f 100644 --- a/libraries/gz_quadruped_playground/launch/ocs2.launch.py +++ b/libraries/gz_quadruped_playground/launch/ocs2.launch.py @@ -1,8 +1,11 @@ import xacro from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.actions import DeclareLaunchArgument, RegisterEventHandler, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.event_handlers import OnProcessExit from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare def generate_launch_description(): joint_state_publisher = Node( @@ -25,9 +28,56 @@ def generate_launch_description(): arguments=["ocs2_quadruped_controller", "--controller-manager", "/controller_manager"] ) + elevation_mapping = Node ( + package="elevation_mapping", + executable="elevation_mapping", + parameters=[ + PathJoinSubstitution( + [ + FindPackageShare("ocs2_quadruped_controller"), + "config", + "elevation_mapping.yaml", + ] + ) + ], + ) + + elevation_mapping = Node ( + package="elevation_mapping", + executable="elevation_mapping", + parameters=[ + PathJoinSubstitution( + [ + FindPackageShare("ocs2_quadruped_controller"), + "config", + "elevation_mapping.yaml", + ] + ) + ], + ) + + convex_plane_decomposition = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare("convex_plane_decomposition_ros"), + "launch", + "convex_plane_decomposition.launch.py" + ]) + ]), + launch_arguments={ + "parameter_file": PathJoinSubstitution([ + FindPackageShare("legged_perceptive_controllers"), + "config", + "convex_plane_decomposition.yaml" + ]) + }.items() + ) + return LaunchDescription([ joint_state_publisher, + # elevation_mapping, + # convex_plane_decomposition, RegisterEventHandler( event_handler=OnProcessExit( target_action=joint_state_publisher, diff --git a/libraries/gz_quadruped_playground/models/D435/model.xacro b/libraries/gz_quadruped_playground/models/D435/model.xacro index 137dfc7..6962b81 100644 --- a/libraries/gz_quadruped_playground/models/D435/model.xacro +++ b/libraries/gz_quadruped_playground/models/D435/model.xacro @@ -80,10 +80,6 @@ 30 true rgbd_${name} - -