From a7fe0df4c565db15710581bdcba1d33d70aa0ddb Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 19 Jul 2024 11:47:21 +0200 Subject: [PATCH 01/23] Fix constrained-based planning / PoseModelStateSpace (#2910) * Fix constrained-based planning Constrained-based planning enables the PoseModelStateSpace by default, meaning that state samples are created by sampling in SE(3) and then performing IK. This state space frequently caused issues, eventually leading to the introduction of the ROS parameter enforce_joint_model_state_space in PR #541 as a work-around. A more detailed analysis shows that the state space's interpolation approach is the culprit - performing interpolation in Cartesian space. If the two interpolation ends are close in Cartesian but distant in joint space, the interpolated state (obtained from IK) can be far away from both start and end (particularly for redundant arms), while its Cartesian pose is naturally close to both interpolation states. This will lead to consecutive waypoints being close in Cartesian space but distant in joint space. If such a trajectory is executed, the controller will interpolate in joint space, leading to a large and unchecked motion. To avoid this issue, interpolation should be performed in joint space during planning as well. The only drawback of this change is that trajectories are less linear in Cartesian space. However, as the trajectory is random-sampled anyway, this shouldn't be a big issue. * PoseModelStateSpace: Use joint-space distance Configurations may be similar in Cartesian EE pose, but very far apart in joint space. What actually matters when connecting states is the joint-space distance. --- .../work_space/pose_model_state_space.h | 1 - .../work_space/pose_model_state_space.cpp | 42 +------------------ 2 files changed, 2 insertions(+), 41 deletions(-) diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h index 4ebe272e8d..6f7837ae3a 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h @@ -145,6 +145,5 @@ class PoseModelStateSpace : public ModelBasedStateSpace }; std::vector poses_; - double jump_factor_; }; } // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp index 1325a13638..12d95371ab 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp @@ -54,8 +54,6 @@ const std::string PoseModelStateSpace::PARAMETERIZATION_TYPE = "PoseModel"; PoseModelStateSpace::PoseModelStateSpace(const ModelBasedStateSpaceSpecification& spec) : ModelBasedStateSpace(spec) { - jump_factor_ = 3; // \todo make this a param - if (spec.joint_model_group_->getGroupKinematics().first) { poses_.emplace_back(spec.joint_model_group_, spec.joint_model_group_->getGroupKinematics().first); @@ -82,10 +80,7 @@ PoseModelStateSpace::~PoseModelStateSpace() = default; double PoseModelStateSpace::distance(const ompl::base::State* state1, const ompl::base::State* state2) const { - double total = 0; - for (std::size_t i = 0; i < poses_.size(); ++i) - total += poses_[i].state_space_->distance(state1->as()->poses[i], state2->as()->poses[i]); - return total; + return ModelBasedStateSpace::distance(state1, state2); } double PoseModelStateSpace::getMaximumExtent() const @@ -136,42 +131,9 @@ void PoseModelStateSpace::sanityChecks() const void PoseModelStateSpace::interpolate(const ompl::base::State* from, const ompl::base::State* to, const double t, ompl::base::State* state) const { - // we want to interpolate in Cartesian space; we do not have a guarantee that from and to - // have their poses computed, but this is very unlikely to happen (depends how the planner gets its input states) - // interpolate in joint space ModelBasedStateSpace::interpolate(from, to, t, state); - - // interpolate SE3 components - for (std::size_t i = 0; i < poses_.size(); ++i) - { - poses_[i].state_space_->interpolate(from->as()->poses[i], to->as()->poses[i], t, - state->as()->poses[i]); - } - - // the call above may reset all flags for state; but we know the pose we want flag should be set - state->as()->setPoseComputed(true); - - /* - std::cout << "*********** interpolate\n"; - printState(from, std::cout); - printState(to, std::cout); - printState(state, std::cout); - std::cout << "\n\n"; - */ - - // after interpolation we cannot be sure about the joint values (we use them as seed only) - // so we recompute IK if needed - if (computeStateIK(state)) - { - double dj = jump_factor_ * ModelBasedStateSpace::distance(from, to); - double d_from = ModelBasedStateSpace::distance(from, state); - double d_to = ModelBasedStateSpace::distance(state, to); - - // if the joint value jumped too much - if (d_from + d_to > std::max(0.2, dj)) // \todo make 0.2 a param - state->as()->markInvalid(); - } + computeStateFK(state); } void PoseModelStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ) From bc07edf159cb54cdcf4c717a3bf5c02696e1d3c4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ga=C3=ABl=20=C3=89corchard?= Date: Mon, 22 Jul 2024 16:18:53 +0200 Subject: [PATCH 02/23] Copy planning scene predicates in the copy constructor (#2858) --- moveit_core/planning_scene/src/planning_scene.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index eaffc31d42..816bad2f8b 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -210,6 +210,10 @@ PlanningScene::PlanningScene(const PlanningSceneConstPtr& parent) : parent_(pare robot_model_ = parent_->robot_model_; + setStateFeasibilityPredicate(parent->getStateFeasibilityPredicate()); + setMotionFeasibilityPredicate(parent->getMotionFeasibilityPredicate()); + setCollisionObjectUpdateCallback(parent_->current_world_object_update_callback_); + // maintain a separate world. Copy on write ensures that most of the object // info is shared until it is modified. world_ = std::make_shared(*parent_->world_); @@ -1204,6 +1208,8 @@ void PlanningScene::decoupleParent() } } + setCollisionObjectUpdateCallback(nullptr); + parent_.reset(); } From 30fbc31e5368094a5df7515c13528760ddf23054 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 26 Jul 2024 12:55:39 +0200 Subject: [PATCH 03/23] Fix deprecation warning (#2922) --- .../src/pointcloud_octomap_updater.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index 6e2b8678d8..bce0275430 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include @@ -96,7 +97,6 @@ void PointCloudOctomapUpdater::start() if (!ns_.empty()) prefix = ns_ + "/"; - rclcpp::QoS qos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data)); if (!filtered_cloud_topic_.empty()) { filtered_cloud_publisher_ = @@ -107,7 +107,11 @@ void PointCloudOctomapUpdater::start() return; /* subscribe to point cloud topic using tf filter*/ point_cloud_subscriber_ = new message_filters::Subscriber(node_, point_cloud_topic_, +#if RCLCPP_VERSION_GTE(28, 3, 0) + rclcpp::SensorDataQoS()); +#else rmw_qos_profile_sensor_data); +#endif if (tf_listener_ && tf_buffer_ && !monitor_->getMapFrame().empty()) { point_cloud_filter_ = new tf2_ros::MessageFilter( From 1be9a62b0ca31bca2c152d6a6dd9cb139e037735 Mon Sep 17 00:00:00 2001 From: Chris Schindlbeck Date: Fri, 26 Jul 2024 16:01:07 +0200 Subject: [PATCH 04/23] Bump codespell to 2.3.0, Fix various typos in cpp/h files, Sort codespell_words (#2937) --- .codespell_words | 8 ++++---- .pre-commit-config.yaml | 2 +- .../include/moveit/distance_field/find_internal_points.h | 2 +- .../moveit/online_signal_smoothing/butterworth_filter.h | 4 ++-- .../src/trajectory_blender_transition_window.cpp | 2 +- .../planning_scene_monitor/src/planning_scene_monitor.cpp | 2 +- .../moveit/move_group_interface/move_group_interface.h | 2 +- .../moveit_setup_controllers/src/controllers_widget.cpp | 2 +- .../moveit_setup_srdf_plugins/src/robot_poses.cpp | 2 +- 9 files changed, 13 insertions(+), 13 deletions(-) diff --git a/.codespell_words b/.codespell_words index 33d567ff91..6be59c42cd 100644 --- a/.codespell_words +++ b/.codespell_words @@ -1,19 +1,19 @@ +aas ang ans atleast ba +brin dof dur iff keyserver nto ot +padd parameterizes planed +sinic tork uint whis -sinic -padd -brin -aas diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 129be497ef..c8968d84dc 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -47,7 +47,7 @@ repos: files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: ['-fallback-style=none', '-i'] - repo: https://github.com/codespell-project/codespell - rev: v2.2.6 + rev: v2.3.0 hooks: - id: codespell args: ['--write-changes', '--ignore-words=.codespell_words', '--skip="*.eps"'] diff --git a/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h b/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h index f03a42d39f..5af220a81e 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h +++ b/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h @@ -48,7 +48,7 @@ namespace distance_field * * @param [in] body The body to discretize * @param [in] resolution The resolution at which to test - * @param [out] points The points internal to the body are appended to thiss + * @param [out] points The points internal to the body are appended to this * vector. */ void findInternalPointsConvex(const bodies::Body& body, double resolution, EigenSTL::vector_Vector3d& points); diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h index 80212d2fd2..8f5e5b362f 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h @@ -60,7 +60,7 @@ namespace online_signal_smoothing * It comes from finding the bilinear transform equivalent of the analog transfer function and * further applying the inverse z-transform. * The parameter "low_pass_filter_coeff" equals (2*pi / tan(omega_d * T)) - * where omega_d is the cutoff frequency and T is the samping period in sec. + * where omega_d is the cutoff frequency and T is the sampling period in sec. */ class ButterworthFilter { @@ -69,7 +69,7 @@ class ButterworthFilter * Constructor. * @param low_pass_filter_coeff Larger filter_coeff-> more smoothing of commands, but more lag. * low_pass_filter_coeff = (2*pi / tan(omega_d * T)) - * where omega_d is the cutoff frequency and T is the samping period in sec. + * where omega_d is the cutoff frequency and T is the sampling period in sec. */ ButterworthFilter(double low_pass_filter_coeff); ButterworthFilter() = delete; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp index a2998e6ecb..159640da14 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp @@ -274,7 +274,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::searchIn // (last point of the first trajectory, first point of the second trajectory) Eigen::Isometry3d circ_pose = req.first_trajectory->getLastWayPoint().getFrameTransform(req.link_name); - // Searh for intersection points according to distance + // Search for intersection points according to distance if (!linearSearchIntersectionPoint(req.link_name, circ_pose.translation(), req.blend_radius, req.first_trajectory, true, first_interse_index)) { diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 103a7e1d09..106a6fd663 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -866,7 +866,7 @@ void PlanningSceneMonitor::excludeRobotLinksFromOctree() bool warned = false; for (const moveit::core::LinkModel* link : links) { - std::vector shapes = link->getShapes(); // copy shared ptrs on purpuse + std::vector shapes = link->getShapes(); // copy shared ptrs on purpose for (std::size_t j = 0; j < shapes.size(); ++j) { // merge mesh vertices up to 0.1 mm apart diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index f3c7b6f506..c8b673a1b3 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -103,7 +103,7 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface MOVEIT_STRUCT_FORWARD(Plan); - /// The representation of a motion plan (as ROS messasges) + /// The representation of a motion plan (as ROS messages) struct Plan { /// The full starting state used for planning diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/controllers_widget.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/controllers_widget.cpp index e902fc78e7..9580de2b3c 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/src/controllers_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_controllers/src/controllers_widget.cpp @@ -355,7 +355,7 @@ void ControllersWidget::deleteController() // Delete actual controller if (setup_step_->deleteController(controller_name)) { - RCLCPP_INFO_STREAM(setup_step_->getLogger(), "Controller " << controller_name << " deleted succefully"); + RCLCPP_INFO_STREAM(setup_step_->getLogger(), "Controller " << controller_name << " deleted successfully"); } else { diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp index 2d907c92b5..9e9529fbbd 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp @@ -51,7 +51,7 @@ void RobotPoses::onInit() // Set the planning scene // srdf_config_->getPlanningScene()->setName("MoveIt Planning Scene"); - // Collision Detection initializtion ------------------------------- + // Collision Detection initialization ------------------------------- // Setup the request request_.contacts = true; From c6163bfdbf0e56bb67f13b95f004e22b2e299dc6 Mon Sep 17 00:00:00 2001 From: Chris Schindlbeck Date: Fri, 26 Jul 2024 17:03:28 +0200 Subject: [PATCH 05/23] Remove additional word 'hardware' in Moveit Controllers section of MoveIt Setup Assistant (#2936) --- .../include/moveit_setup_controllers/moveit_controllers.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.hpp b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.hpp index 6dc958350e..bcaf78297d 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.hpp +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.hpp @@ -60,8 +60,7 @@ class MoveItControllers : public Controllers std::string getInstructions() const override { - return "Configure controllers to be used in executing trajectories with MoveIt (hardware or simulation)." - "hardware"; + return "Configure controllers to be used in executing trajectories with MoveIt (hardware or simulation)."; } std::string getButtonText() const override From 51c38625b0e935a50230624bd8f7beaae46600d7 Mon Sep 17 00:00:00 2001 From: Shobuj Paul <72087882+Shobuj-Paul@users.noreply.github.com> Date: Fri, 26 Jul 2024 20:36:28 +0530 Subject: [PATCH 06/23] Const topic instead of parameters (#2728) --- .../include/moveit/moveit_cpp/moveit_cpp.h | 25 ++++++++++--------- 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index f3c6edaa7b..15251ccdc9 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -57,27 +57,28 @@ class MoveItCpp /// Specification of options to use when constructing the MoveItCpp class struct PlanningSceneMonitorOptions { + PlanningSceneMonitorOptions() + : joint_state_topic(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC) + , attached_collision_object_topic( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC) + , monitored_planning_scene_topic(planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC) + , publish_planning_scene_topic(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC) + { + } + void load(const rclcpp::Node::SharedPtr& node) { const std::string ns = "planning_scene_monitor_options"; node->get_parameter_or(ns + ".name", name, std::string("planning_scene_monitor")); node->get_parameter_or(ns + ".robot_description", robot_description, std::string("robot_description")); - node->get_parameter_or(ns + ".joint_state_topic", joint_state_topic, - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC); - node->get_parameter_or(ns + ".attached_collision_object_topic", attached_collision_object_topic, - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC); - node->get_parameter_or(ns + ".monitored_planning_scene_topic", monitored_planning_scene_topic, - planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC); - node->get_parameter_or(ns + ".publish_planning_scene_topic", publish_planning_scene_topic, - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC); node->get_parameter_or(ns + ".wait_for_initial_state_timeout", wait_for_initial_state_timeout, 0.0); } std::string name; std::string robot_description; - std::string joint_state_topic; - std::string attached_collision_object_topic; - std::string monitored_planning_scene_topic; - std::string publish_planning_scene_topic; + const std::string joint_state_topic; + const std::string attached_collision_object_topic; + const std::string monitored_planning_scene_topic; + const std::string publish_planning_scene_topic; double wait_for_initial_state_timeout; }; From 64d917eca7e31b25e18eeb434c56ebba3d0da022 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 30 Jul 2024 23:48:06 -0700 Subject: [PATCH 07/23] Add new accessor methods to move group interface (#2925) Signed-off-by: methylDragon --- .../move_group_interface.h | 14 +++++ .../src/move_group_interface.cpp | 55 +++++++++++++------ 2 files changed, 53 insertions(+), 16 deletions(-) diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index c8b673a1b3..87f03685e0 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -236,6 +236,9 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface If the value is greater than 1, it is set to 1.0. */ void setMaxVelocityScalingFactor(double max_velocity_scaling_factor); + /** \brief Get the max velocity scaling factor set by setMaxVelocityScalingFactor(). */ + double getMaxVelocityScalingFactor() const; + /** \brief Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0,1]. The maximum joint acceleration specified in the robot model is multiplied by the factor. If the value is 0, it is set to @@ -243,6 +246,9 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface If the value is greater than 1, it is set to 1.0. */ void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor); + /** \brief Get the max acceleration scaling factor set by setMaxAccelerationScalingFactor(). */ + double getMaxAccelerationScalingFactor() const; + /** \brief Get the number of seconds set by setPlanningTime() */ double getPlanningTime() const; @@ -796,6 +802,14 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface /** \brief How often is the system allowed to move the camera to update environment model when looking */ void setLookAroundAttempts(int32_t attempts); + /** \brief Build a RobotState message for use with plan() or computeCartesianPath() + * If the move_group has a custom set start state, this method will use that as the robot state. + * + * Otherwise, the robot state will be with `is_diff` set to true, causing it to be an offset from the current state + * of the robot at time of the state's use. + */ + void constructRobotState(moveit_msgs::msg::RobotState& state); + /** \brief Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and store it in \e request */ void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request); diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 4f93f72781..028819a611 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -359,11 +359,21 @@ class MoveGroupInterface::MoveGroupInterfaceImpl setMaxScalingFactor(max_velocity_scaling_factor_, value, "velocity_scaling_factor", 0.1); } + double getMaxVelocityScalingFactor() const + { + return max_velocity_scaling_factor_; + } + void setMaxAccelerationScalingFactor(double value) { setMaxScalingFactor(max_acceleration_scaling_factor_, value, "acceleration_scaling_factor", 0.1); } + double getMaxAccelerationScalingFactor() const + { + return max_acceleration_scaling_factor_; + } + void setMaxScalingFactor(double& variable, const double target_value, const char* factor_name, double fallback_value) { if (target_value > 1.0) @@ -857,14 +867,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl auto req = std::make_shared(); moveit_msgs::srv::GetCartesianPath::Response::SharedPtr response; - if (considered_start_state_) - { - moveit::core::robotStateToRobotStateMsg(*considered_start_state_, req->start_state); - } - else - { - req->start_state.is_diff = true; - } + constructRobotState(req->start_state); req->group_name = opt_.group_name; req->header.frame_id = getPoseReferenceFrame(); @@ -1009,6 +1012,18 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return allowed_planning_time_; } + void constructRobotState(moveit_msgs::msg::RobotState& state) const + { + if (considered_start_state_) + { + moveit::core::robotStateToRobotStateMsg(*considered_start_state_, state); + } + else + { + state.is_diff = true; + } + } + void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request) const { request.group_name = opt_.group_name; @@ -1020,14 +1035,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl request.planner_id = planner_id_; request.workspace_parameters = workspace_parameters_; - if (considered_start_state_) - { - moveit::core::robotStateToRobotStateMsg(*considered_start_state_, request.start_state); - } - else - { - request.start_state.is_diff = true; - } + constructRobotState(request.start_state); if (active_target_ == JOINT) { @@ -1441,11 +1449,21 @@ void MoveGroupInterface::setMaxVelocityScalingFactor(double max_velocity_scaling impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor); } +double MoveGroupInterface::getMaxVelocityScalingFactor() const +{ + return impl_->getMaxVelocityScalingFactor(); +} + void MoveGroupInterface::setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor) { impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor); } +double MoveGroupInterface::getMaxAccelerationScalingFactor() const +{ + return impl_->getMaxAccelerationScalingFactor(); +} + moveit::core::MoveItErrorCode MoveGroupInterface::asyncMove() { return impl_->move(false); @@ -2326,6 +2344,11 @@ bool MoveGroupInterface::detachObject(const std::string& name) return impl_->detachObject(name); } +void MoveGroupInterface::constructRobotState(moveit_msgs::msg::RobotState& state) +{ + impl_->constructRobotState(state); +} + void MoveGroupInterface::constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& goal_out) { impl_->constructMotionPlanRequest(goal_out); From 97ea2044fdfa4454c9feba512484c2592320dcc6 Mon Sep 17 00:00:00 2001 From: FSund Date: Wed, 31 Jul 2024 10:08:56 +0200 Subject: [PATCH 08/23] Propagate "clear octomap" actions to monitoring planning scenes (#3134) Set a non-empty octomap.id if the octomap was cleared and perform an octomap update if that id is non-empty. Co-authored-by: Robert Haschke # Conflicts: # moveit_core/planning_scene/src/planning_scene.cpp # moveit_core/planning_scene/test/test_planning_scene.cpp --- .../planning_scene/src/planning_scene.cpp | 7 ++- .../test/test_planning_scene.cpp | 55 +++++++++++++++++++ 2 files changed, 60 insertions(+), 2 deletions(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 816bad2f8b..bf1a83eead 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -654,7 +654,10 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& sce { if (it.first == OCTOMAP_NS) { - do_omap = true; + if (it.second == collision_detection::World::DESTROY) + scene_msg.world.octomap.octomap.id = "cleared"; // indicate cleared octomap + else + do_omap = true; } else if (it.second == collision_detection::World::DESTROY) { @@ -1260,7 +1263,7 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScen result &= processCollisionObjectMsg(collision_object); // if an octomap was specified, replace the one we have with that one - if (!scene_msg.world.octomap.octomap.data.empty()) + if (!scene_msg.world.octomap.octomap.id.empty()) processOctomapMsg(scene_msg.world.octomap); return result; diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index 4bd5c25ce9..909f3f6576 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -44,6 +44,8 @@ #include #include #include +#include +#include #include #include @@ -89,6 +91,59 @@ TEST(PlanningScene, LoadRestore) EXPECT_EQ(ps.getRobotModel()->getName(), ps_msg.robot_model_name); } +TEST(PlanningScene, LoadOctomap) +{ + urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2"); + srdf::ModelSharedPtr srdf_model(new srdf::Model()); + planning_scene::PlanningScene ps(urdf_model, srdf_model); + + { // check octomap before doing any operations on it + octomap_msgs::msg::OctomapWithPose msg; + ps.getOctomapMsg(msg); + EXPECT_TRUE(msg.octomap.id.empty()); + EXPECT_TRUE(msg.octomap.data.empty()); + } + + { // fill PlanningScene's octomap + octomap::OcTree octomap(0.1); + octomap::point3d origin(0, 0, 0); + octomap::point3d end(0, 1, 2); + octomap.insertRay(origin, end); + + // populate PlanningScene with octomap + moveit_msgs::msg::PlanningScene msg; + msg.is_diff = true; + octomap_msgs::fullMapToMsg(octomap, msg.world.octomap.octomap); + ps.setPlanningSceneDiffMsg(msg); + + // validate octomap message + octomap_msgs::msg::OctomapWithPose octomap_msg; + ps.getOctomapMsg(octomap_msg); + EXPECT_EQ(octomap_msg.octomap.id, "OcTree"); + EXPECT_EQ(octomap_msg.octomap.data.size(), msg.world.octomap.octomap.data.size()); + } + + { // verify that a PlanningScene msg with an empty octomap id does not modify the octomap + // create planning scene + moveit_msgs::msg::PlanningScene msg; + msg.is_diff = true; + ps.setPlanningSceneDiffMsg(msg); + + octomap_msgs::msg::OctomapWithPose octomap_msg; + ps.getOctomapMsg(octomap_msg); + EXPECT_EQ(octomap_msg.octomap.id, "OcTree"); + EXPECT_FALSE(octomap_msg.octomap.data.empty()); + } + + { // check that a non-empty octomap id, but empty octomap will clear the octomap + moveit_msgs::msg::PlanningScene msg; + msg.is_diff = true; + msg.world.octomap.octomap.id = "xxx"; + ps.setPlanningSceneDiffMsg(msg); + EXPECT_FALSE(static_cast(ps.getWorld()->getObject(planning_scene::PlanningScene::OCTOMAP_NS))); + } +} + TEST(PlanningScene, LoadRestoreDiff) { urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2"); From 72a6e92c25dc36a7b710c72ba38915ed8b1aef70 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 31 Jul 2024 10:14:45 +0200 Subject: [PATCH 09/23] Silent "empty quaternion" warning from poseMsgToEigen() (#3435) # Conflicts: # moveit_core/planning_scene/src/planning_scene.cpp --- .../planning_scene/src/planning_scene.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index bf1a83eead..479f3426ce 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1726,16 +1726,16 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs:: shapes.reserve(num_shapes); shape_poses.reserve(num_shapes); - utilities::poseMsgToEigen(object.pose, object_pose); - bool switch_object_pose_and_shape_pose = false; - if (num_shapes == 1) + if (num_shapes == 1 && moveit::core::isEmpty(object.pose)) { - if (moveit::core::isEmpty(object.pose)) - { - switch_object_pose_and_shape_pose = true; // If the object pose is not set but the shape pose is, - // use the shape's pose as the object pose. - } + // If the object pose is not set but the shape pose is, use the shape's pose as the object pose. + switch_object_pose_and_shape_pose = true; + object_pose.setIdentity(); + } + else + { + utilities::poseMsgToEigen(object.pose, object_pose); } auto append = [&object_pose, &shapes, &shape_poses, From 47681f3e24f5c0d85fcf62246e1680639e87bb5d Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Wed, 15 May 2024 03:49:57 -0400 Subject: [PATCH 10/23] Allow moving of all shapes of an object in one go (#3599) --- .../moveit/collision_detection/world.h | 3 ++ moveit_core/collision_detection/src/world.cpp | 20 ++++++++++ .../planning_scene/src/planning_scene.cpp | 40 +++++++++++++++++++ 3 files changed, 63 insertions(+) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.h b/moveit_core/collision_detection/include/moveit/collision_detection/world.h index 92799537c5..d610d4b43c 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h @@ -217,6 +217,9 @@ class World bool moveShapeInObject(const std::string& object_id, const shapes::ShapeConstPtr& shape, const Eigen::Isometry3d& shape_pose); + /** \brief Update the pose of all shapes in an object. Shape size is verified. Returns true on success. */ + bool moveShapesInObject(const std::string& object_id, const EigenSTL::vector_Isometry3d& shape_poses); + /** \brief Move the object pose (thus moving all shapes and subframes in the object) * according to the given transform specified in world frame. * The transform is relative to and changes the object pose. It does not replace it. diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp index eae07cf01c..ce7c6006fc 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -259,6 +259,26 @@ bool World::moveShapeInObject(const std::string& object_id, const shapes::ShapeC return false; } +bool World::moveShapesInObject(const std::string& object_id, const EigenSTL::vector_Isometry3d& shape_poses) +{ + auto it = objects_.find(object_id); + if (it != objects_.end()) + { + if (shape_poses.size() == it->second->shapes_.size()) + { + for (std::size_t i = 0; i < shape_poses.size(); ++i) + { + ASSERT_ISOMETRY(shape_poses[i]) // unsanitized input, could contain a non-isometry + it->second->shape_poses_[i] = shape_poses[i]; + it->second->global_shape_poses_[i] = it->second->pose_ * shape_poses[i]; + } + notify(it->second, MOVE_SHAPE); + return true; + } + } + return false; +} + bool World::moveObject(const std::string& object_id, const Eigen::Isometry3d& transform) { const auto it = objects_.find(object_id); diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 479f3426ce..9dbcab0add 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1857,6 +1857,7 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision { if (world_->hasObject(object.id)) { + // update object pose if (!object.primitives.empty() || !object.meshes.empty() || !object.planes.empty()) { RCLCPP_WARN(getLogger(), "Move operation for object '%s' ignores the geometry specified in the message.", @@ -1870,6 +1871,45 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision const Eigen::Isometry3d object_frame_transform = world_to_object_header_transform * header_to_pose_transform; world_->setObjectPose(object.id, object_frame_transform); + + // update shape poses + if (!object.primitive_poses.empty() || !object.mesh_poses.empty() || !object.plane_poses.empty()) + { + auto world_object = world_->getObject(object.id); // object exists, checked earlier + + std::size_t shape_size = object.primitive_poses.size() + object.mesh_poses.size() + object.plane_poses.size(); + if (shape_size != world_object->shape_poses_.size()) + { + ROS_ERROR_NAMED(LOGNAME, "Move operation for object '%s' must have same number of geometry poses. Cannot move.", + object.id.c_str()); + return false; + } + + // order matters -> primitive, mesh and plane + EigenSTL::vector_Isometry3d shape_poses; + for (const auto& shape_pose : object.primitive_poses) + { + shape_poses.emplace_back(); + PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back()); + } + for (const auto& shape_pose : object.mesh_poses) + { + shape_poses.emplace_back(); + PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back()); + } + for (const auto& shape_pose : object.plane_poses) + { + shape_poses.emplace_back(); + PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back()); + } + + if (!world_->moveShapesInObject(object.id, shape_poses)) + { + ROS_ERROR_NAMED(LOGNAME, "Move operation for object '%s' internal world error. Cannot move.", object.id.c_str()); + return false; + } + } + return true; } From 1d93035fc4fbc2831916bce26d7b03e15b8a86d5 Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Fri, 19 Jul 2024 06:49:45 -0400 Subject: [PATCH 11/23] Optimize MOVE_SHAPE operations for FCL (#3601) Don't recreate the FCL object when moving shapes, but just update its transforms. --- .../test_collision_common_panda.h | 13 ++++++++ .../src/collision_env_fcl.cpp | 32 +++++++++++++++++++ .../planning_scene/src/planning_scene.cpp | 18 +++++++---- 3 files changed, 57 insertions(+), 6 deletions(-) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h index 92163406aa..061a304085 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -155,11 +155,24 @@ TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_1) ASSERT_TRUE(res.collision); res.clear(); + pos1.translation().z() = 0.25; + this->cenv_->getWorld()->moveObject("box", pos1); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + pos1.translation().z() = 0.05; this->cenv_->getWorld()->moveObject("box", pos1); this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); ASSERT_TRUE(res.collision); res.clear(); + pos1.translation().z() = 0.25; + this->cenv_->getWorld()->moveObject("box", pos1); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + res.clear(); + this->cenv_->getWorld()->moveObject("box", pos1); ASSERT_FALSE(res.collision); } diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp index 54a007d325..86a48b8320 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -450,6 +450,38 @@ void CollisionEnvFCL::notifyObjectChange(const ObjectConstPtr& obj, World::Actio } cleanCollisionGeometryCache(); } + else if (action == World::MOVE_SHAPE) + { + auto it = fcl_objs_.find(obj->id_); + if (it == fcl_objs_.end()) + { + RCLCPP_ERROR(getLogger(), "Cannot move shapes of unknown FCL object: '%s'", obj->id_.c_str()); + return; + } + + if (obj->global_shape_poses_.size() != it->second.collision_objects_.size()) + { + RCLCPP_ERROR(getLogger(), + "Cannot move shapes, shape size mismatch between FCL object and world object: '%s'. Respectively " + "%zu and %zu.", + obj->id_.c_str(), it->second.collision_objects_.size(), it->second.collision_objects_.size()); + return; + } + + for (std::size_t i = 0; i < it->second.collision_objects_.size(); ++i) + { + it->second.collision_objects_[i]->setTransform(transform2fcl(obj->global_shape_poses_[i])); + + // compute AABB, order matters + it->second.collision_geometry_[i]->collision_geometry_->computeLocalAABB(); + it->second.collision_objects_[i]->computeAABB(); + } + + // update AABB in the FCL broadphase manager tree + // see https://github.com/moveit/moveit/pull/3601 for benchmarks + it->second.unregisterFrom(manager_.get()); + it->second.registerTo(manager_.get()); + } else { updateFCLObject(obj->id_); diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 9dbcab0add..0b955ac888 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -655,9 +655,13 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& sce if (it.first == OCTOMAP_NS) { if (it.second == collision_detection::World::DESTROY) + { scene_msg.world.octomap.octomap.id = "cleared"; // indicate cleared octomap + } else + { do_omap = true; + } } else if (it.second == collision_detection::World::DESTROY) { @@ -1880,8 +1884,9 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision std::size_t shape_size = object.primitive_poses.size() + object.mesh_poses.size() + object.plane_poses.size(); if (shape_size != world_object->shape_poses_.size()) { - ROS_ERROR_NAMED(LOGNAME, "Move operation for object '%s' must have same number of geometry poses. Cannot move.", - object.id.c_str()); + RCLCPP_ERROR(getLogger(), + "Move operation for object '%s' must have same number of geometry poses. Cannot move.", + object.id.c_str()); return false; } @@ -1890,22 +1895,23 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision for (const auto& shape_pose : object.primitive_poses) { shape_poses.emplace_back(); - PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back()); + utilities::poseMsgToEigen(shape_pose, shape_poses.back()); } for (const auto& shape_pose : object.mesh_poses) { shape_poses.emplace_back(); - PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back()); + utilities::poseMsgToEigen(shape_pose, shape_poses.back()); } for (const auto& shape_pose : object.plane_poses) { shape_poses.emplace_back(); - PlanningScene::poseMsgToEigen(shape_pose, shape_poses.back()); + utilities::poseMsgToEigen(shape_pose, shape_poses.back()); } if (!world_->moveShapesInObject(object.id, shape_poses)) { - ROS_ERROR_NAMED(LOGNAME, "Move operation for object '%s' internal world error. Cannot move.", object.id.c_str()); + RCLCPP_ERROR(getLogger(), "Move operation for object '%s' internal world error. Cannot move.", + object.id.c_str()); return false; } } From bb4b0b8280a17f1920b7deb6dfa447f1adbedf57 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 19 Jul 2024 11:55:05 +0200 Subject: [PATCH 12/23] Fix CHOMP segfault (#3621) * Prevent segfault when getParentLinkModel() is NULL * Simpliy code * Add unit test operating the panda gripper --------- Co-authored-by: Captain Yoshi --- .../chomp_interface/test/chomp_moveit.test | 7 -- .../test/chomp_moveit_panda.test | 9 +++ .../test/chomp_moveit_rrbot.test | 7 ++ .../test/chomp_moveit_test_panda.cpp | 80 +++++++++++++++++++ ...t_test.cpp => chomp_moveit_test_rrbot.cpp} | 2 +- .../src/chomp_optimizer.cpp | 27 ++----- 6 files changed, 105 insertions(+), 27 deletions(-) delete mode 100644 moveit_planners/chomp/chomp_interface/test/chomp_moveit.test create mode 100644 moveit_planners/chomp/chomp_interface/test/chomp_moveit_panda.test create mode 100644 moveit_planners/chomp/chomp_interface/test/chomp_moveit_rrbot.test create mode 100644 moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_panda.cpp rename moveit_planners/chomp/chomp_interface/test/{chomp_moveit_test.cpp => chomp_moveit_test_rrbot.cpp} (98%) diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit.test b/moveit_planners/chomp/chomp_interface/test/chomp_moveit.test deleted file mode 100644 index e965ff64b3..0000000000 --- a/moveit_planners/chomp/chomp_interface/test/chomp_moveit.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_panda.test b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_panda.test new file mode 100644 index 0000000000..952f150350 --- /dev/null +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_panda.test @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_rrbot.test b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_rrbot.test new file mode 100644 index 0000000000..47c4dfe61b --- /dev/null +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_rrbot.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_panda.cpp b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_panda.cpp new file mode 100644 index 0000000000..0d84b43521 --- /dev/null +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_panda.cpp @@ -0,0 +1,80 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Sherbrooke University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/// \author Captain Yoshi + +#include +#include + +#include +#include + +class CHOMPMoveitTest : public ::testing::Test +{ +public: + moveit::planning_interface::MoveGroupInterface move_group_; + moveit::planning_interface::MoveGroupInterface::Plan my_plan_; + +public: + CHOMPMoveitTest() : move_group_("hand") + { + } +}; + +// TEST CASES + +// https://github.com/moveit/moveit/issues/2542 +TEST_F(CHOMPMoveitTest, jointSpaceGoodGoal) +{ + move_group_.setStartState(*(move_group_.getCurrentState())); + move_group_.setJointValueTarget(std::vector({ 0.0, 0.0 })); + move_group_.setPlanningTime(5.0); + + moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_); + EXPECT_GT(my_plan_.trajectory_.joint_trajectory.points.size(), 0u); + EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::SUCCESS); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "chomp_moveit_test_panda"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_rrbot.cpp similarity index 98% rename from moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp rename to moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_rrbot.cpp index 8e79efa70b..7e69f51c41 100644 --- a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_rrbot.cpp @@ -107,7 +107,7 @@ TEST_F(CHOMPMoveitTest, collisionAtEndOfPath) int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "chomp_moveit_test"); + ros::init(argc, argv, "chomp_moveit_test_rrbot"); ros::AsyncSpinner spinner(1); spinner.start(); diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp index d1fc2c2091..23ead931c0 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp @@ -205,26 +205,15 @@ void ChompOptimizer::initialize() { if (fixed_link_resolution_map.find(link->getParentJointModel()->getName()) == fixed_link_resolution_map.end()) { - const moveit::core::JointModel* parent_model = nullptr; - bool found_root = false; - - while (!found_root) + const moveit::core::JointModel* parent_model = link->getParentJointModel(); + while (true) // traverse up the tree until we find a joint we know about in joint_names_ { - if (parent_model == nullptr) - { - parent_model = link->getParentJointModel(); - } - else - { - parent_model = parent_model->getParentLinkModel()->getParentJointModel(); - for (const std::string& joint_name : joint_names_) - { - if (parent_model->getName() == joint_name) - { - found_root = true; - } - } - } + if (parent_model->getParentLinkModel() == nullptr) + break; + + parent_model = parent_model->getParentLinkModel()->getParentJointModel(); + if (std::find(joint_names_.begin(), joint_names_.end(), parent_model->getName()) != joint_names_.end()) + break; } fixed_link_resolution_map[link->getParentJointModel()->getName()] = parent_model->getName(); } From ef4041e315a0779128b871b313ecb477f2eb367b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 19 Jul 2024 12:47:46 +0200 Subject: [PATCH 13/23] CHOMP: Fix handling of mimic joints (#3625) As the CHOMP planner only considers active joints, it needs to use setJointGroupActivePositions() instead of setJointGroupPositions(). --- .../chomp/chomp_motion_planner/src/chomp_optimizer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp index 23ead931c0..f31f48510d 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp @@ -952,7 +952,7 @@ void ChompOptimizer::setRobotStateFromPoint(ChompTrajectory& group_trajectory, i for (size_t j = 0; j < group_trajectory.getNumJoints(); ++j) joint_states.emplace_back(point(0, j)); - state_.setJointGroupPositions(planning_group_, joint_states); + state_.setJointGroupActivePositions(planning_group_, joint_states); state_.update(); } From 00f6c6fa6aad0000f279f25901660ccbf82482d1 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Fri, 2 Aug 2024 10:26:48 -0400 Subject: [PATCH 14/23] Deduplicate joint trajectory points in Pilz Move Group Sequence capability (#2943) * Deduplicate joint trajectory points before sending them to controllers * Fix max loop value * Move deduplication code to Pilz * Clean up * Add todo --- .../robot_trajectory/robot_trajectory.h | 11 ++++++++++ .../cartesian_trajectory_point.h | 1 - .../src/command_list_manager.cpp | 21 ++++++++++++++++++- 3 files changed, 31 insertions(+), 2 deletions(-) diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index 1f77946234..ed2887e2a2 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -244,6 +244,17 @@ class RobotTrajectory void swap(robot_trajectory::RobotTrajectory& other) noexcept; + /** + * \brief Remove a point from the trajectory + * \param index - the index to remove + */ + RobotTrajectory& removeWayPoint(std::size_t index) + { + waypoints_.erase(waypoints_.begin() + index); + duration_from_previous_.erase(duration_from_previous_.begin() + index); + return *this; + } + RobotTrajectory& clear() { waypoints_.clear(); diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h index 4835dc8876..e810ae4726 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h @@ -38,7 +38,6 @@ #include #include -#include namespace pilz_industrial_motion_planner { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp index be11aa84da..de829de34a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp @@ -112,7 +112,26 @@ RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConst // therefore: "i-1". (i > 0 ? radii.at(i - 1) : 0.)); } - return plan_comp_builder_.build(); + + const auto res_vec = plan_comp_builder_.build(); + + // De-duplicate trajectory points with the same time value. + // This is necessary since some controllers do not allow times that are not monotonically increasing. + // TODO: Ideally, we would not need this code if the trajectory segments were created without + // duplicate time points in the first place. Leaving this note to revisit this with a more principled fix. + for (const auto& traj : res_vec) + { + for (size_t i = 0; i < traj->size() - 1; ++i) + { + if (traj->getWayPointDurationFromStart(i) == traj->getWayPointDurationFromStart(i + 1)) + { + RCLCPP_WARN(getLogger(), "Removed duplicate point at time=%f", traj->getWayPointDurationFromStart(i)); + traj->removeWayPoint(i + 1); + } + } + } + + return res_vec; } bool CommandListManager::checkRadiiForOverlap(const robot_trajectory::RobotTrajectory& traj_A, const double radii_A, From d7f4d482515087d6b103b868853d1e0102339aaa Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 2 Aug 2024 12:06:37 -0500 Subject: [PATCH 15/23] Tune Servo params so it does not get stuck so easily (#2939) --- moveit_ros/moveit_servo/config/panda_simulated_config.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index b5d57bfcb9..5037999513 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml @@ -12,7 +12,7 @@ max_expected_latency: 0.1 # delay between sending a command and the robot execu command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" - linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + linear: 0.2 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. joint: 0.5 @@ -41,9 +41,9 @@ check_octomap_collisions: false # Check collision against the octomap (if a 3D move_group_name: panda_arm # Often 'manipulator' or 'arm' ## Configure handling of singularities and joint limits -lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) +lower_singularity_threshold: 10.0 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this -joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # added as a buffer to joint limits [radians or meters]. If moving quickly, make this larger. +joint_limit_margins: [0.12, 0.12, 0.12, 0.12, 0.12, 0.12, 0.12] # added as a buffer to joint limits [radians or meters]. If moving quickly, make this larger. leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/moveit/moveit2/pull/620) ## Topic names From d7b84fd82f69542538a28863d699f1dd7f4fdc86 Mon Sep 17 00:00:00 2001 From: Chris Schindlbeck Date: Fri, 2 Aug 2024 19:07:23 +0200 Subject: [PATCH 16/23] Replace deprecated load_yaml with xacro.load_yaml in ros2_control.xacro template (#2934) --- .../templates/config/ros2_control.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_setup_assistant/moveit_setup_controllers/templates/config/ros2_control.xacro b/moveit_setup_assistant/moveit_setup_controllers/templates/config/ros2_control.xacro index 3dcb96d963..c28dcc4710 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/templates/config/ros2_control.xacro +++ b/moveit_setup_assistant/moveit_setup_controllers/templates/config/ros2_control.xacro @@ -1,7 +1,7 @@ - + From 57995c5692cf4456fe0c88b1eb1b6ecfde77bfb8 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Fri, 2 Aug 2024 10:08:09 -0700 Subject: [PATCH 17/23] Add TF Buffer accessor to MoveGroupInterface (#2944) --- .../moveit/move_group_interface/move_group_interface.h | 3 +++ .../move_group_interface/src/move_group_interface.cpp | 5 +++++ 2 files changed, 8 insertions(+) diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index 87f03685e0..4f82643017 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -160,6 +160,9 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface * states from srdf */ const std::vector& getNamedTargets() const; + /** \brief Get the tf2_ros::Buffer. */ + const std::shared_ptr& getTF() const; + /** \brief Get the RobotModel object. */ moveit::core::RobotModelConstPtr getRobotModel() const; diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 028819a611..3fbf9e3c63 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -1382,6 +1382,11 @@ const std::vector& MoveGroupInterface::getNamedTargets() const return impl_->getJointModelGroup()->getDefaultStateNames(); } +const std::shared_ptr& MoveGroupInterface::getTF() const +{ + return impl_->getTF(); +} + moveit::core::RobotModelConstPtr MoveGroupInterface::getRobotModel() const { return impl_->getRobotModel(); From 19d4a2818bca7024305cbf5be5c3bca4186351d5 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Fri, 2 Aug 2024 19:08:53 +0200 Subject: [PATCH 18/23] =?UTF-8?q?=F0=9F=9B=A0=EF=B8=8F=20Bump=20SonarSourc?= =?UTF-8?q?e/sonarcloud-github-c-cpp=20from=202=20to=203=20(#2883)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/sonar.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/sonar.yaml b/.github/workflows/sonar.yaml index 083d6c07ca..8febc84e0c 100644 --- a/.github/workflows/sonar.yaml +++ b/.github/workflows/sonar.yaml @@ -110,7 +110,7 @@ jobs: run: cat ${{ github.workspace }}/.work/target_ws/coverage.info - name: Install sonar-scanner if: steps.ici.outputs.target_test_results == '0' - uses: SonarSource/sonarcloud-github-c-cpp@v2 + uses: SonarSource/sonarcloud-github-c-cpp@v3 - name: Run sonar-scanner if: steps.ici.outputs.target_test_results == '0' env: From fc7993318045b5d604ad9dd3352def90d414b5b6 Mon Sep 17 00:00:00 2001 From: Abhiroop Bhavsar <126786356+akky20@users.noreply.github.com> Date: Tue, 6 Aug 2024 17:35:06 +0530 Subject: [PATCH 19/23] fixed typo from trajactory to trajectory (#2951) Signed-off-by: Abhiroop Bhavsar --- moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp index 5997f1cadf..40a037675a 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp @@ -171,7 +171,7 @@ void initMoveitPy(py::module& m) Returns the planning scene monitor. )") - .def("get_trajactory_execution_manager", &moveit_cpp::MoveItCpp::getTrajectoryExecutionManagerNonConst, + .def("get_trajectory_execution_manager", &moveit_cpp::MoveItCpp::getTrajectoryExecutionManagerNonConst, py::return_value_policy::reference, R"( Returns the trajectory execution manager. From 38c1fd00ad60fb3a58608d111c4213ebcc4fa109 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 6 Aug 2024 15:13:19 +0200 Subject: [PATCH 20/23] Don't set reset observer callback & set CB after world_ is initialized (#2950) --- moveit_core/planning_scene/src/planning_scene.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 0b955ac888..8655117fb8 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -212,12 +212,12 @@ PlanningScene::PlanningScene(const PlanningSceneConstPtr& parent) : parent_(pare setStateFeasibilityPredicate(parent->getStateFeasibilityPredicate()); setMotionFeasibilityPredicate(parent->getMotionFeasibilityPredicate()); - setCollisionObjectUpdateCallback(parent_->current_world_object_update_callback_); // maintain a separate world. Copy on write ensures that most of the object // info is shared until it is modified. world_ = std::make_shared(*parent_->world_); world_const_ = world_; + setCollisionObjectUpdateCallback(parent_->current_world_object_update_callback_); // record changes to the world world_diff_ = std::make_shared(world_); @@ -1214,9 +1214,6 @@ void PlanningScene::decoupleParent() (object_types_.value())[it->first] = it->second; } } - - setCollisionObjectUpdateCallback(nullptr); - parent_.reset(); } From 4ad84ccc5a1541e150609f1dd783185c8cbc0737 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Tue, 6 Aug 2024 10:27:47 -0400 Subject: [PATCH 21/23] Install Gazebo in tutorial image Dockerfile (#2952) * Install Gazebo in tutorial image Dockerfile * Add Jazzy job --------- Co-authored-by: Sebastian Jahr --- .docker/tutorial-source/Dockerfile | 11 ++++++++++- .github/workflows/tutorial_docker.yaml | 2 +- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/.docker/tutorial-source/Dockerfile b/.docker/tutorial-source/Dockerfile index e4f875e91b..933513c375 100644 --- a/.docker/tutorial-source/Dockerfile +++ b/.docker/tutorial-source/Dockerfile @@ -1,12 +1,16 @@ # syntax = docker/dockerfile:1.3 # ghcr.io/moveit/moveit2:main-${ROS_DISTRO}-tutorial-source -# Source build of the repos file from the tutorail site +# Source build of the repos file from the tutorial site ARG ROS_DISTRO=rolling +ARG GZ_VERSION=harmonic + FROM moveit/moveit2:${ROS_DISTRO}-ci LABEL maintainer Tyler Weaver tyler@picknik.ai +ARG GZ_VERSION + # Export ROS_UNDERLAY for downstream docker containers ENV ROS_UNDERLAY /root/ws_moveit/install WORKDIR $ROS_UNDERLAY/.. @@ -17,6 +21,11 @@ COPY . src/moveit2 # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size # https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#minimize-the-number-of-layers RUN --mount=type=cache,target=/root/.ccache/,sharing=locked \ + # Install Gazebo, which is needed by some dependencies. + sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' && \ + wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - && \ + sudo apt update && \ + sudo apt-get install -y "gz-${GZ_VERSION}" && \ # Enable ccache PATH=/usr/lib/ccache:$PATH && \ # Checkout the tutorial repo diff --git a/.github/workflows/tutorial_docker.yaml b/.github/workflows/tutorial_docker.yaml index 2438e4750f..cd305eec20 100644 --- a/.github/workflows/tutorial_docker.yaml +++ b/.github/workflows/tutorial_docker.yaml @@ -16,7 +16,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling, humble, iron] + ROS_DISTRO: [rolling, humble, iron, jazzy] runs-on: ubuntu-latest permissions: packages: write From f30d51b2cef111ca92c89ced8776960d6e3b2470 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20Garc=C3=ADa=20L=C3=B3pez?= Date: Wed, 7 Aug 2024 17:03:36 +0200 Subject: [PATCH 22/23] Consider attached collision objects in the ComputeFK service (#2953) --- .../default_capabilities/kinematics_service_capability.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp index 6101ab60cc..c2cd031305 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp @@ -223,10 +223,10 @@ bool MoveGroupKinematicsService::computeFKService(const std::shared_ptrrobot_state, rs); for (std::size_t i = 0; i < req->fk_link_names.size(); ++i) { - if (rs.getRobotModel()->hasLinkModel(req->fk_link_names[i])) + if (rs.knowsFrameTransform(req->fk_link_names[i])) { res->pose_stamped.resize(res->pose_stamped.size() + 1); - res->pose_stamped.back().pose = tf2::toMsg(rs.getGlobalLinkTransform(req->fk_link_names[i])); + res->pose_stamped.back().pose = tf2::toMsg(rs.getFrameTransform(req->fk_link_names[i])); res->pose_stamped.back().header.frame_id = default_frame; res->pose_stamped.back().header.stamp = context_->moveit_cpp_->getNode()->get_clock()->now(); if (do_transform) From 50310747e4eb0c8a50c3eb866d2b017033416465 Mon Sep 17 00:00:00 2001 From: Abhiroop Bhavsar <126786356+akky20@users.noreply.github.com> Date: Thu, 8 Aug 2024 19:39:53 +0530 Subject: [PATCH 23/23] Correct moveit logo path in readme.md file (#2957) * Correct moveit logo path in readme.md file Signed-off-by: Abhiroop Bhavsar * Correct moveit logo path in readme.md file (black color) Signed-off-by: Abhiroop Bhavsar * Update link README.md Co-authored-by: Robert Haschke --------- Signed-off-by: Abhiroop Bhavsar Co-authored-by: Robert Haschke --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index fa3d75cc09..9504b3e69c 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -MoveIt Logo +MoveIt Logo The [MoveIt Motion Planning Framework for ROS 2](http://moveit.ros.org). For the ROS 1 repository see [MoveIt 1](https://github.com/moveit/moveit). For the commercially supported version see [MoveIt Pro](http://picknik.ai/pro).