diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml index 312b728102..d4afb59d83 100644 --- a/.github/workflows/docker.yaml +++ b/.github/workflows/docker.yaml @@ -46,7 +46,7 @@ jobs: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - name: Build and Push - uses: docker/build-push-action@v5 + uses: docker/build-push-action@v6 if: ${{ github.event_name == 'workflow_dispatch' || github.event_name != 'schedule' || steps.apt.outputs.no_cache }} with: file: .docker/${{ github.job }}/Dockerfile @@ -90,7 +90,7 @@ jobs: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - name: Build and Push - uses: docker/build-push-action@v5 + uses: docker/build-push-action@v6 if: ${{ github.event_name == 'workflow_dispatch' || github.event_name != 'schedule' || steps.apt.outputs.no_cache }} with: file: .docker/${{ github.job }}/Dockerfile @@ -136,7 +136,7 @@ jobs: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} - name: Build and Push - uses: docker/build-push-action@v5 + uses: docker/build-push-action@v6 if: ${{ github.event_name == 'workflow_dispatch' || github.event_name != 'schedule' || steps.apt.outputs.no_cache }} with: file: .docker/${{ github.job }}/Dockerfile @@ -174,7 +174,7 @@ jobs: - name: "Remove .dockerignore" run: rm .dockerignore # enforce full source context - name: Build and Push - uses: docker/build-push-action@v5 + uses: docker/build-push-action@v6 with: context: . file: .docker/${{ github.job }}/Dockerfile diff --git a/README.md b/README.md index 97b6893d06..ba01fe9d69 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -MoveIt Logo +MoveIt Logo The [MoveIt Motion Planning Framework for ROS](http://moveit.ros.org). For the ROS 2 repository see [MoveIt 2](https://github.com/moveit/moveit2). For the commercially supported version see [MoveIt Pro](http://picknik.ai/pro). diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst index d1e66f7118..6660fefe26 100644 --- a/moveit/CHANGELOG.rst +++ b/moveit/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- + 1.1.14 (2024-05-27) ------------------- diff --git a/moveit/package.xml b/moveit/package.xml index aecb0d0a68..027da9bc76 100644 --- a/moveit/package.xml +++ b/moveit/package.xml @@ -1,7 +1,7 @@ moveit - 1.1.14 + 1.1.15 Meta package that contains all essential package of MoveIt. Until Summer 2016 MoveIt had been developed over multiple repositories, where developers' usability and maintenance effort was non-trivial. See the detailed discussion for the merge of several repositories. Dave Coleman Michael Ferguson diff --git a/moveit_commander/CHANGELOG.rst b/moveit_commander/CHANGELOG.rst index fa0df68c59..e57237aeaa 100644 --- a/moveit_commander/CHANGELOG.rst +++ b/moveit_commander/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_commander ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- +* New implementation for computeCartesianPath (`#3618 `_) +* Contributors: Robert Haschke + 1.1.14 (2024-05-27) ------------------- * Fixed order of return values in doc string of compute_cartesian_path() (`#3574 `_) diff --git a/moveit_commander/demos/plan_with_constraints.py b/moveit_commander/demos/plan_with_constraints.py index 01b69ed1a1..6ab57f5a95 100755 --- a/moveit_commander/demos/plan_with_constraints.py +++ b/moveit_commander/demos/plan_with_constraints.py @@ -71,5 +71,5 @@ wpose.position.y += 0.05 waypoints.append(wpose) - plan, fraction = a.compute_cartesian_path(waypoints, 0.01, 0.0, path_constraints=c) + plan, fraction = a.compute_cartesian_path(waypoints, 0.01, True, path_constraints=c) print("Plan success percent: ", fraction) diff --git a/moveit_commander/package.xml b/moveit_commander/package.xml index edc9fc606a..5bde2a80d1 100644 --- a/moveit_commander/package.xml +++ b/moveit_commander/package.xml @@ -1,7 +1,7 @@ moveit_commander - 1.1.14 + 1.1.15 Python interfaces to MoveIt Ioan Sucan diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py index da3a4dfc46..a5509f9253 100644 --- a/moveit_commander/src/moveit_commander/move_group.py +++ b/moveit_commander/src/moveit_commander/move_group.py @@ -648,7 +648,6 @@ def compute_cartesian_path( self, waypoints, eef_step, - jump_threshold, avoid_collisions=True, path_constraints=None, ): @@ -670,7 +669,6 @@ def compute_cartesian_path( (ser_path, fraction) = self._g.compute_cartesian_path( [conversions.pose_to_list(p) for p in waypoints], eef_step, - jump_threshold, avoid_collisions, constraints_str, ) @@ -678,7 +676,6 @@ def compute_cartesian_path( (ser_path, fraction) = self._g.compute_cartesian_path( [conversions.pose_to_list(p) for p in waypoints], eef_step, - jump_threshold, avoid_collisions, ) diff --git a/moveit_commander/test/python_time_parameterization.py b/moveit_commander/test/python_time_parameterization.py index 1cf587d6af..c373ea9c16 100755 --- a/moveit_commander/test/python_time_parameterization.py +++ b/moveit_commander/test/python_time_parameterization.py @@ -60,7 +60,7 @@ def plan(self): goal_pose = self.group.get_current_pose().pose goal_pose.position.z -= 0.1 (plan, fraction) = self.group.compute_cartesian_path( - [start_pose, goal_pose], 0.005, 0.0 + [start_pose, goal_pose], 0.005 ) self.assertEqual(fraction, 1.0, "Cartesian path plan failed") return plan diff --git a/moveit_core/CHANGELOG.rst b/moveit_core/CHANGELOG.rst index 1ea7511cdb..098947bb94 100644 --- a/moveit_core/CHANGELOG.rst +++ b/moveit_core/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package moveit_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- +* Disable ruckig's webclient support in MoveIt build (`#3636 `_) +* New implementation for computeCartesianPath (`#3618 `_) +* Optimize MOVE_SHAPE operations for FCL (`#3601 `_) +* Provide violated bounds for a JointConstraint +* PSM: Correctly handle full planning scene message (`#3610 `_) +* Contributors: Captain Yoshi, Michael Görner, Robert Haschke + 1.1.14 (2024-05-27) ------------------- * Allow moving of all shapes of an object in one go (`#3599 `_) diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index e7ffb293c5..1c6eda6b29 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -50,6 +50,13 @@ find_package(ruckig REQUIRED) # in include_directories below because the target is correctly imported here. get_target_property(ruckig_INCLUDE_DIRS ruckig::ruckig INTERFACE_INCLUDE_DIRECTORIES) set(ruckig_LIBRARIES "ruckig::ruckig") +# Remove the WITH_CLOUD_CLIENT definition because we don't use it and it causes build issues in some configurations +# See https://github.com/moveit/moveit/pull/3636 +get_target_property(ruckig_defs ruckig::ruckig INTERFACE_COMPILE_DEFINITIONS) +if(ruckig_defs) + list(REMOVE_ITEM ruckig_defs "WITH_CLOUD_CLIENT") + set_target_properties(ruckig::ruckig PROPERTIES INTERFACE_COMPILE_DEFINITIONS "${ruckig_defs}") +endif() find_package(urdfdom REQUIRED) find_package(urdfdom_headers REQUIRED) 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 ac2c2ebf45..435eee6f0d 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 48019f70dc..82530c9e71 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -434,6 +434,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()) + { + ROS_ERROR_NAMED(LOGNAME, "Cannot move shapes of unknown FCL object: '%s'", obj->id_.c_str()); + return; + } + + if (obj->global_shape_poses_.size() != it->second.collision_objects_.size()) + { + ROS_ERROR_NAMED(LOGNAME, + "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/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 8b13c24e5b..36afdb9bc9 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -221,18 +221,18 @@ bool JointConstraint::configure(const moveit_msgs::JointConstraint& jc) joint_position_ = bounds.min_position_; joint_tolerance_above_ = std::numeric_limits::epsilon(); ROS_WARN_NAMED("kinematic_constraints", - "Joint %s is constrained to be below the minimum bounds. " + "Joint %s is constrained to be below the minimum bound %g. " "Assuming minimum bounds instead.", - jc.joint_name.c_str()); + jc.joint_name.c_str(), bounds.min_position_); } else if (bounds.max_position_ < joint_position_ - joint_tolerance_below_) { joint_position_ = bounds.max_position_; joint_tolerance_below_ = std::numeric_limits::epsilon(); ROS_WARN_NAMED("kinematic_constraints", - "Joint %s is constrained to be above the maximum bounds. " + "Joint %s is constrained to be above the maximum bounds %g. " "Assuming maximum bounds instead.", - jc.joint_name.c_str()); + jc.joint_name.c_str(), bounds.max_position_); } } diff --git a/moveit_core/package.xml b/moveit_core/package.xml index ada3560b99..f15f49c8f2 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -1,7 +1,7 @@ moveit_core - 1.1.14 + 1.1.15 Core libraries used by MoveIt Ioan Sucan Sachin Chitta diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 4b5d57b701..332d41f4e0 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -1318,6 +1318,7 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene& sc bool PlanningScene::setPlanningSceneMsg(const moveit_msgs::PlanningScene& scene_msg) { + assert(scene_msg.is_diff == false); ROS_DEBUG_NAMED(LOGNAME, "Setting new planning scene: '%s'", scene_msg.name.c_str()); name_ = scene_msg.name; diff --git a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h index 40ed1163ce..0a6de5c207 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h +++ b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h @@ -46,6 +46,14 @@ namespace moveit { namespace core { +/** Struct defining linear and rotational precision */ +struct CartesianPrecision +{ + double translational = 0.001; //< max deviation in translation (meters) + double rotational = 0.01; //< max deviation in rotation (radians) + double max_resolution = 1e-5; //< max resolution for waypoints (fraction of total path) +}; + /** \brief Struct for containing jump_threshold. For the purposes of maintaining API, we support both \e jump_threshold_factor which provides a scaling factor for @@ -97,6 +105,75 @@ class CartesianInterpolator // TODO(mlautman): Eventually, this planner should be moved out of robot_state public: + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular link. + + The Cartesian path to be followed is specified as a \e translation vector to be followed by the robot \e link. + This vector is assumed to be specified either in the global reference frame or in the local + reference frame of the link. + The resulting joint values are stored in the vector \e traj, one by one. The interpolation distance in + Cartesian space between consecutive points on the resulting path is specified in the \e MaxEEFStep struct which + provides two fields: translation and rotation. If a \e validCallback is specified, this is passed to the internal + call to setFromIK(). In case of IK failure, the computation of the path stops and the value returned corresponds to + the distance that was achieved and for which corresponding states were added to the path. + + The struct CartesianPrecision specifies the precision to which the path should follow the Cartesian straight line. + If the deviation at the mid point of two consecutive waypoints is larger than the specified precision, another waypoint + will be inserted at that mid point. The precision is specified separately for translation and rotation. + The maximal resolution to consider (as fraction of the total path length) is specified by max_resolution. + */ + static double + computeCartesianPath(const RobotState* start_state, const JointModelGroup* group, + std::vector>& traj, const LinkModel* link, + const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step, + const CartesianPrecision& precision, + const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); + + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular link. + + In contrast to the previous function, the translation vector is specified as a (unit) direction vector and + a distance. */ + static double + computeCartesianPath(const RobotState* start_state, const JointModelGroup* group, + std::vector>& traj, const LinkModel* link, + const Eigen::Vector3d& direction, bool global_reference_frame, double distance, + const MaxEEFStep& max_step, const CartesianPrecision& precision, + const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) + { + return computeCartesianPath(start_state, group, traj, link, distance * direction, global_reference_frame, max_step, + precision, validCallback, options); + } + + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular frame. + + In contrast to the previous function, the Cartesian path is specified as a target frame to be reached (\e target) + for a virtual frame attached to the robot \e link with the given \e link_offset. + The target frame is assumed to be specified either w.r.t. to the global reference frame or the virtual link frame. + This function returns the fraction (0..1) of path that was achieved. All other comments from the previous function apply. */ + static double computeCartesianPath(const RobotState* start_state, const JointModelGroup* group, + std::vector& traj, const LinkModel* link, + const Eigen::Isometry3d& target, bool global_reference_frame, + const MaxEEFStep& max_step, const CartesianPrecision& precision, + const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options, + const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity()); + + /** \brief Compute the sequence of joint values that perform a general Cartesian path. + + In contrast to the previous functions, the Cartesian path is specified as a set of \e waypoints to be sequentially + reached by the virtual frame attached to the robot \e link. The waypoints are transforms given either w.r.t. the global + reference frame or the virtual frame at the immediately preceding waypoint. The virtual frame needs + to move in a straight line between two consecutive waypoints. All other comments apply. */ + static double + computeCartesianPath(const RobotState* start_state, const JointModelGroup* group, + std::vector>& traj, const LinkModel* link, + const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, + const MaxEEFStep& max_step, const CartesianPrecision& precision, + const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity()); + /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular link. The Cartesian path to be followed is specified as a \e translation vector to be followed by the robot \e link. @@ -125,7 +202,7 @@ class CartesianInterpolator For absolute jump thresholds, if any individual joint-space motion delta is larger then \e revolute_jump_threshold for revolute joints or \e prismatic_jump_threshold for prismatic joints then this step is considered a failure and the returned path is truncated up to just before the jump.*/ - static double + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static double computeCartesianPath(RobotState* start_state, const JointModelGroup* group, std::vector>& traj, const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step, @@ -137,7 +214,7 @@ class CartesianInterpolator In contrast to the previous function, the translation vector is specified as a (unit) direction vector and a distance. */ - static double + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static double computeCartesianPath(RobotState* start_state, const JointModelGroup* group, std::vector>& traj, const LinkModel* link, const Eigen::Vector3d& direction, bool global_reference_frame, double distance, @@ -145,8 +222,11 @@ class CartesianInterpolator const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" return computeCartesianPath(start_state, group, traj, link, distance * direction, global_reference_frame, max_step, jump_threshold, validCallback, options); +#pragma GCC diagnostic pop } /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular frame. @@ -155,7 +235,7 @@ class CartesianInterpolator for a virtual frame attached to the robot \e link with the given \e link_offset. The target frame is assumed to be specified either w.r.t. to the global reference frame or the virtual link frame. This function returns the fraction (0..1) of path that was achieved. All other comments from the previous function apply. */ - static double + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static double computeCartesianPath(RobotState* start_state, const JointModelGroup* group, std::vector>& traj, const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step, @@ -170,7 +250,7 @@ class CartesianInterpolator reached by the virtual frame attached to the robot \e link. The waypoints are transforms given either w.r.t. the global reference frame or the virtual frame at the immediately preceding waypoint. The virtual frame needs to move in a straight line between two consecutive waypoints. All other comments apply. */ - static double + [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static double computeCartesianPath(RobotState* start_state, const JointModelGroup* group, std::vector>& traj, const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h index d07fea302c..4462b1ed03 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h +++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h @@ -1122,78 +1122,6 @@ class RobotState bool setFromDiffIK(const JointModelGroup* group, const geometry_msgs::Twist& twist, const std::string& tip, double dt, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn()); - /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path for a particular group. - - The Cartesian path to be followed is specified as a direction of motion (\e direction, unit vector) for the origin - The Cartesian path to be followed is specified as a direction of motion (\e direction, unit vector) for the origin - of a robot link (\e link). The direction is assumed to be either in a global reference frame or in the local - reference frame of the link. In the latter case (\e global_reference_frame is false) the \e direction is rotated - accordingly. The link needs to move in a straight line, following the specified direction, for the desired \e - distance. The resulting joint values are stored in the vector \e traj, one by one. The maximum distance in - Cartesian space between consecutive points on the resulting path is specified in the \e MaxEEFStep struct which - provides two fields: translation and rotation. If a \e validCallback is specified, this is passed to the internal - call to setFromIK(). In case of IK failure, the computation of the path stops and the value returned corresponds to - the distance that was computed and for which corresponding states were added to the path. At the end of the - function call, the state of the group corresponds to the last attempted Cartesian pose. - - During the computation of the trajectory, it is usually preferred if consecutive joint values do not 'jump' by a - large amount in joint space, even if the Cartesian distance between the corresponding points is small as expected. - To account for this, the \e jump_threshold struct is provided, which comprises three fields: - \e jump_threshold_factor, \e revolute_jump_threshold and \e prismatic_jump_threshold. - If either \e revolute_jump_threshold or \e prismatic_jump_threshold are non-zero, we test for absolute jumps. - If \e jump_threshold_factor is non-zero, we test for relative jumps. Otherwise (all params are zero), jump - detection is disabled. - - For relative jump detection, the average joint-space distance between consecutive points in the trajectory is - computed. If any individual joint-space motion delta is larger then this average distance by a factor of - \e jump_threshold_factor, this step is considered a failure and the returned path is truncated up to just - before the jump. - - For absolute jump thresholds, if any individual joint-space motion delta is larger then \e revolute_jump_threshold - for revolute joints or \e prismatic_jump_threshold for prismatic joints then this step is considered a failure and - the returned path is truncated up to just before the jump. - - NOTE: As of ROS-Melodic these are deprecated and should not be used - */ - [[deprecated]] double - computeCartesianPath(const JointModelGroup* group, std::vector& traj, const LinkModel* link, - const Eigen::Vector3d& direction, bool global_reference_frame, double distance, double max_step, - double jump_threshold_factor, - const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); - - /** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular group. - - In contrast to the previous function, the Cartesian path is specified as a target frame to be reached (\e target) - for the origin of a robot link (\e link). The target frame is assumed to be either in a global reference frame or - in the local reference frame of the link. In the latter case (\e global_reference_frame is false) the \e target is - rotated accordingly. All other comments from the previous function apply. - - NOTE: As of ROS-Melodic these are deprecated and should not be used - */ - [[deprecated]] double - computeCartesianPath(const JointModelGroup* group, std::vector& traj, const LinkModel* link, - const Eigen::Isometry3d& target, bool global_reference_frame, double max_step, - double jump_threshold_factor, - const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); - - /** \brief Compute the sequence of joint values that perform a general Cartesian path. - - In contrast to the previous functions, the Cartesian path is specified as a set of \e waypoints to be sequentially - reached for the origin of a robot link (\e link). The waypoints are transforms given either in a global reference - frame or in the local reference frame of the link at the immediately preceeding waypoint. The link needs to move - in a straight line between two consecutive waypoints. All other comments apply. - - NOTE: As of ROS-Melodic these are deprecated and should not be used - */ - [[deprecated]] double - computeCartesianPath(const JointModelGroup* group, std::vector& traj, const LinkModel* link, - const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, double max_step, - double jump_threshold_factor, - const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(), - const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()); - /** \brief Compute the Jacobian with reference to a particular point on a given link, for a specified group. * \param group The group to compute the Jacobian for * \param link The link model to compute the Jacobian for diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index 4d8d99f68f..2d87ae98fd 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -34,7 +34,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman */ +/* Author: Ioan Sucan, Sachin Chitta, Acorn Pooley, Mario Prats, Dave Coleman, Robert Haschke */ #include #include @@ -52,6 +52,173 @@ static const std::size_t MIN_STEPS_FOR_JUMP_THRESH = 10; const std::string LOGNAME = "cartesian_interpolator"; +bool validateAndImproveInterval(const RobotState& start_state, const RobotState& end_state, + const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& end_pose, + std::vector& traj, double& percentage, const double width, + const JointModelGroup* group, const LinkModel* link, + const CartesianPrecision& precision, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options, const Eigen::Isometry3d& link_offset) +{ + // compute pose at joint-space midpoint between start_state and end_state + RobotState mid_state(start_state.getRobotModel()); + start_state.interpolate(end_state, 0.5, mid_state); + Eigen::Isometry3d fk_pose = mid_state.getGlobalLinkTransform(link) * link_offset; + + // compute pose at Cartesian-space midpoint between start_pose and end_pose + Eigen::Isometry3d mid_pose(Eigen::Quaterniond(start_pose.linear()).slerp(0.5, Eigen::Quaterniond(end_pose.linear()))); + mid_pose.translation() = 0.5 * (start_pose.translation() + end_pose.translation()); + + // if deviation between both poses, fk_pose and mid_pose is within precision, we are satisfied + double linear_distance = (mid_pose.translation() - fk_pose.translation()).norm(); + double angular_distance = Eigen::Quaterniond(mid_pose.linear()).angularDistance(Eigen::Quaterniond(fk_pose.linear())); + if (linear_distance <= precision.translational && angular_distance <= precision.rotational) + { + traj.push_back(std::make_shared(end_state)); + return true; + } + + if (width < precision.max_resolution) + return false; // failed to find linear interpolation within max_resolution + + // otherwise subdivide interval further, computing IK for mid_pose + if (!mid_state.setFromIK(group, mid_pose * link_offset.inverse(), link->getName(), 0.0, validCallback, options)) + return false; + + // and recursively processing the two sub-intervals + const auto half_width = width / 2.0; + const auto old_percentage = percentage; + percentage = percentage - half_width; + if (!validateAndImproveInterval(start_state, mid_state, start_pose, mid_pose, traj, percentage, half_width, group, + link, precision, validCallback, options, link_offset)) + return false; + + percentage = old_percentage; + return validateAndImproveInterval(mid_state, end_state, mid_pose, end_pose, traj, percentage, half_width, group, link, + precision, validCallback, options, link_offset); +} + +double CartesianInterpolator::computeCartesianPath(const RobotState* start_state, const JointModelGroup* group, + std::vector& traj, const LinkModel* link, + const Eigen::Vector3d& translation, bool global_reference_frame, + const MaxEEFStep& max_step, const CartesianPrecision& precision, + const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options) +{ + const double distance = translation.norm(); + // The target pose is obtained by adding the translation vector to the link's current pose + Eigen::Isometry3d pose = start_state->getGlobalLinkTransform(link); + + // the translation direction can be specified w.r.t. the local link frame (then rotate into global frame) + pose.translation() += global_reference_frame ? translation : pose.linear() * translation; + + // call computeCartesianPath for the computed target pose in the global reference frame + return distance * + computeCartesianPath(start_state, group, traj, link, pose, true, max_step, precision, validCallback, options); +} + +double CartesianInterpolator::computeCartesianPath(const RobotState* start_state, const JointModelGroup* group, + std::vector& traj, const LinkModel* link, + const Eigen::Isometry3d& target, bool global_reference_frame, + const MaxEEFStep& max_step, const CartesianPrecision& precision, + const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options, + const Eigen::Isometry3d& link_offset) +{ + // check unsanitized inputs for non-isometry + ASSERT_ISOMETRY(target) + ASSERT_ISOMETRY(link_offset) + + RobotState state(*start_state); + + const std::vector& cjnt = group->getContinuousJointModels(); + // make sure that continuous joints wrap + for (const JointModel* joint : cjnt) + state.enforceBounds(joint); + + // Cartesian pose we start from + Eigen::Isometry3d start_pose = state.getGlobalLinkTransform(link) * link_offset; + Eigen::Isometry3d inv_offset = link_offset.inverse(); + + // the target can be in the local reference frame (in which case we rotate it) + Eigen::Isometry3d rotated_target = global_reference_frame ? target : start_pose * target; + + Eigen::Quaterniond start_quaternion(start_pose.linear()); + Eigen::Quaterniond target_quaternion(rotated_target.linear()); + + double rotation_distance = start_quaternion.angularDistance(target_quaternion); + double translation_distance = (rotated_target.translation() - start_pose.translation()).norm(); + + // decide how many steps we will need for this trajectory + std::size_t translation_steps = 0; + if (max_step.translation > 0.0) + translation_steps = floor(translation_distance / max_step.translation); + + std::size_t rotation_steps = 0; + if (max_step.rotation > 0.0) + rotation_steps = floor(rotation_distance / max_step.rotation); + std::size_t steps = std::max(translation_steps, rotation_steps) + 1; + + traj.clear(); + traj.push_back(std::make_shared(*start_state)); + + double last_valid_percentage = 0.0; + Eigen::Isometry3d prev_pose = start_pose; + RobotState prev_state(state); + for (std::size_t i = 1; i <= steps; ++i) + { + double percentage = (double)i / (double)steps; + + Eigen::Isometry3d pose(start_quaternion.slerp(percentage, target_quaternion)); + pose.translation() = percentage * rotated_target.translation() + (1 - percentage) * start_pose.translation(); + + if (!state.setFromIK(group, pose * inv_offset, link->getName(), 0.0, validCallback, options) || + !validateAndImproveInterval(prev_state, state, prev_pose, pose, traj, percentage, 1.0 / (double)steps, group, + link, precision, validCallback, options, link_offset)) + break; + + prev_pose = pose; + prev_state = state; + last_valid_percentage = percentage; + } + + return last_valid_percentage; +} + +double CartesianInterpolator::computeCartesianPath( + const RobotState* start_state, const JointModelGroup* group, std::vector& traj, + const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, + const MaxEEFStep& max_step, const CartesianPrecision& precision, const GroupStateValidityCallbackFn& validCallback, + const kinematics::KinematicsQueryOptions& options, const Eigen::Isometry3d& link_offset) +{ + double percentage_solved = 0.0; + for (std::size_t i = 0; i < waypoints.size(); ++i) + { + std::vector waypoint_traj; + double wp_percentage_solved = + computeCartesianPath(start_state, group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step, + precision, validCallback, options, link_offset); + if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits::epsilon()) + { + percentage_solved = (double)(i + 1) / (double)waypoints.size(); + std::vector::iterator start = waypoint_traj.begin(); + if (i > 0 && !waypoint_traj.empty()) + std::advance(start, 1); + traj.insert(traj.end(), start, waypoint_traj.end()); + } + else + { + percentage_solved += wp_percentage_solved / (double)waypoints.size(); + std::vector::iterator start = waypoint_traj.begin(); + if (i > 0 && !waypoint_traj.empty()) + std::advance(start, 1); + traj.insert(traj.end(), start, waypoint_traj.end()); + break; + } + } + + return percentage_solved; +} + double CartesianInterpolator::computeCartesianPath(RobotState* start_state, const JointModelGroup* group, std::vector& traj, const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame, @@ -66,9 +233,12 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons // the translation direction can be specified w.r.t. the local link frame (then rotate into global frame) pose.translation() += global_reference_frame ? translation : pose.linear() * translation; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" // call computeCartesianPath for the computed target pose in the global reference frame return distance * computeCartesianPath(start_state, group, traj, link, pose, true, max_step, jump_threshold, validCallback, options); +#pragma GCC diagnostic pop } double CartesianInterpolator::computeCartesianPath(RobotState* start_state, const JointModelGroup* group, @@ -184,9 +354,12 @@ double CartesianInterpolator::computeCartesianPath( // Don't test joint space jumps for every waypoint, test them later on the whole trajectory. static const JumpThreshold NO_JOINT_SPACE_JUMP_TEST; std::vector waypoint_traj; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" double wp_percentage_solved = computeCartesianPath(start_state, group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step, NO_JOINT_SPACE_JUMP_TEST, validCallback, options, link_offset); +#pragma GCC diagnostic pop if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits::epsilon()) { percentage_solved = (double)(i + 1) / (double)waypoints.size(); diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 5c455acbb1..ab357ffa01 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -2027,39 +2027,6 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL:: return false; } -double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector& traj, - const LinkModel* link, const Eigen::Vector3d& direction, - bool global_reference_frame, double distance, double max_step, - double jump_threshold_factor, const GroupStateValidityCallbackFn& validCallback, - const kinematics::KinematicsQueryOptions& options) -{ - return CartesianInterpolator::computeCartesianPath(this, group, traj, link, direction, global_reference_frame, - distance, MaxEEFStep(max_step), - JumpThreshold(jump_threshold_factor), validCallback, options); -} - -double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector& traj, - const LinkModel* link, const Eigen::Isometry3d& target, - bool global_reference_frame, double max_step, double jump_threshold_factor, - const GroupStateValidityCallbackFn& validCallback, - const kinematics::KinematicsQueryOptions& options) -{ - return CartesianInterpolator::computeCartesianPath(this, group, traj, link, target, global_reference_frame, - MaxEEFStep(max_step), JumpThreshold(jump_threshold_factor), - validCallback, options); -} - -double RobotState::computeCartesianPath(const JointModelGroup* group, std::vector& traj, - const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, - bool global_reference_frame, double max_step, double jump_threshold_factor, - const GroupStateValidityCallbackFn& validCallback, - const kinematics::KinematicsQueryOptions& options) -{ - return CartesianInterpolator::computeCartesianPath(this, group, traj, link, waypoints, global_reference_frame, - MaxEEFStep(max_step), JumpThreshold(jump_threshold_factor), - validCallback, options); -} - void RobotState::computeAABB(std::vector& aabb) const { BOOST_VERIFY(checkLinkTransforms()); diff --git a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp index 5f6426709a..8f876aff3c 100644 --- a/moveit_core/robot_state/test/test_cartesian_interpolator.cpp +++ b/moveit_core/robot_state/test/test_cartesian_interpolator.cpp @@ -246,14 +246,16 @@ class PandaRobot : public testing::Test bool global) { return CartesianInterpolator::computeCartesianPath(start_state_.get(), jmg_, result, link_, translation, global, - MaxEEFStep(0.1), JumpThreshold(), GroupStateValidityCallbackFn(), + MaxEEFStep(0.1), CartesianPrecision{}, + GroupStateValidityCallbackFn(), kinematics::KinematicsQueryOptions()); } double computeCartesianPath(std::vector>& result, const Eigen::Isometry3d& target, bool global, const Eigen::Isometry3d& offset = Eigen::Isometry3d::Identity()) { return CartesianInterpolator::computeCartesianPath(start_state_.get(), jmg_, result, link_, target, global, - MaxEEFStep(0.1), JumpThreshold(), GroupStateValidityCallbackFn(), + MaxEEFStep(0.1), CartesianPrecision{ 0.01, 0.01 }, + GroupStateValidityCallbackFn(), kinematics::KinematicsQueryOptions(), offset); } diff --git a/moveit_kinematics/CHANGELOG.rst b/moveit_kinematics/CHANGELOG.rst index 647ac656db..88d2817b91 100644 --- a/moveit_kinematics/CHANGELOG.rst +++ b/moveit_kinematics/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_kinematics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- + 1.1.14 (2024-05-27) ------------------- * Addressing discrepancies in angle parameter definition between IKFast solvers and the plugin template (`#3489 `_) diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index 9b078f55b8..a70de90e63 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -1,7 +1,7 @@ moveit_kinematics - 1.1.14 + 1.1.15 Package for all inverse kinematics solvers in MoveIt Dave Coleman diff --git a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst index e1e8255afd..b6c8070cae 100644 --- a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package chomp_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- +* Fix CHOMP segfault (`#3621 `_) +* Contributors: Robert Haschke, Captain Yoshi + 1.1.14 (2024-05-27) ------------------- diff --git a/moveit_planners/chomp/chomp_interface/CMakeLists.txt b/moveit_planners/chomp/chomp_interface/CMakeLists.txt index 63c741773b..780546d842 100644 --- a/moveit_planners/chomp/chomp_interface/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_interface/CMakeLists.txt @@ -58,10 +58,17 @@ install(TARGETS ${PROJECT_NAME} chomp_planner_plugin if(CATKIN_ENABLE_TESTING) # additional test dependencies find_package(rostest REQUIRED) - add_rostest_gtest(chomp_moveit_test - test/chomp_moveit.test - test/chomp_moveit_test.cpp) - target_link_libraries(chomp_moveit_test + add_rostest_gtest(chomp_moveit_test_rrbot + test/chomp_moveit_rrbot.test + test/chomp_moveit_test_rrbot.cpp) + target_link_libraries(chomp_moveit_test_rrbot + ${catkin_LIBRARIES} + ${rostest_LIBRARIES}) + + add_rostest_gtest(chomp_moveit_test_panda + test/chomp_moveit_panda.test + test/chomp_moveit_test_panda.cpp) + target_link_libraries(chomp_moveit_test_panda ${catkin_LIBRARIES} ${rostest_LIBRARIES}) endif() diff --git a/moveit_planners/chomp/chomp_interface/package.xml b/moveit_planners/chomp/chomp_interface/package.xml index f4bb1211c7..cba750901c 100644 --- a/moveit_planners/chomp/chomp_interface/package.xml +++ b/moveit_planners/chomp/chomp_interface/package.xml @@ -2,7 +2,7 @@ moveit_planners_chomp The interface for using CHOMP within MoveIt - 1.1.14 + 1.1.15 Gil Jones Chittaranjan Srinivas Swaminathan 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 7ea3e9ba65..cd1a47992b 100644 --- a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_rrbot.cpp @@ -108,7 +108,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/CHANGELOG.rst b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst index d545c5a03f..c246848a1d 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package chomp_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- +* CHOMP: Fix handling of mimic joints (`#3625 `_) +* Fix CHOMP segfault (`#3621 `_) +* Contributors: Robert Haschke + 1.1.14 (2024-05-27) ------------------- diff --git a/moveit_planners/chomp/chomp_motion_planner/package.xml b/moveit_planners/chomp/chomp_motion_planner/package.xml index 8d533e7b98..78393b0931 100644 --- a/moveit_planners/chomp/chomp_motion_planner/package.xml +++ b/moveit_planners/chomp/chomp_motion_planner/package.xml @@ -2,7 +2,7 @@ chomp_motion_planner chomp_motion_planner - 1.1.14 + 1.1.15 Gil Jones Mrinal Kalakrishnan 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 8ffc509f5a..2c942cc2d0 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp @@ -208,26 +208,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(); } @@ -1009,7 +998,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(); } diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst index df70f27f05..9d97e72a31 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_chomp_optimizer_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- + 1.1.14 (2024-05-27) ------------------- diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml index dcdb416b25..c57c5ee7dd 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml +++ b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml @@ -3,7 +3,7 @@ moveit_chomp_optimizer_adapter MoveIt planning request adapter utilizing chomp for solution optimization - 1.1.14 + 1.1.15 Raghavender Sahdev Raghavender Sahdev MoveIt Release Team diff --git a/moveit_planners/moveit_planners/CHANGELOG.rst b/moveit_planners/moveit_planners/CHANGELOG.rst index 431ffc9770..be717fc628 100644 --- a/moveit_planners/moveit_planners/CHANGELOG.rst +++ b/moveit_planners/moveit_planners/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_planners ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- + 1.1.14 (2024-05-27) ------------------- diff --git a/moveit_planners/moveit_planners/package.xml b/moveit_planners/moveit_planners/package.xml index 3312bd8d5f..4f5c3fc3eb 100644 --- a/moveit_planners/moveit_planners/package.xml +++ b/moveit_planners/moveit_planners/package.xml @@ -1,6 +1,6 @@ moveit_planners - 1.1.14 + 1.1.15 Metapacakge that installs all available planners for MoveIt Ioan Sucan Sachin Chitta diff --git a/moveit_planners/ompl/CHANGELOG.rst b/moveit_planners/ompl/CHANGELOG.rst index edcf170f45..26b8386ae0 100644 --- a/moveit_planners/ompl/CHANGELOG.rst +++ b/moveit_planners/ompl/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_planners_ompl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- +* Fix constrained-based planning / PoseModelStateSpace (`#3615 `_) +* Contributors: Robert Haschke + 1.1.14 (2024-05-27) ------------------- * Support ompl::ompl cmake target (`#3549 `_) 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 5adb82bb44..15571ce99f 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 @@ -137,6 +137,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 8f050071ae..6db78529ca 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 @@ -50,8 +50,6 @@ const std::string ompl_interface::PoseModelStateSpace::PARAMETERIZATION_TYPE = " ompl_interface::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); else if (!spec.joint_model_group_->getGroupKinematics().second.empty()) @@ -73,10 +71,7 @@ ompl_interface::PoseModelStateSpace::~PoseModelStateSpace() = default; double ompl_interface::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 ompl_interface::PoseModelStateSpace::getMaximumExtent() const @@ -128,42 +123,11 @@ void ompl_interface::PoseModelStateSpace::sanityChecks() const void ompl_interface::PoseModelStateSpace::interpolate(const ompl::base::State* from, const ompl::base::State* to, const double t, ompl::base::State* state) const { - // moveit::Profiler::ScopedBlock sblock("interpolate"); - - // 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) + // moveit::Profiler::ScopedBlock sblock("interpolate"); // 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 ompl_interface::PoseModelStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY, diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml index 64ba28eb4b..40e51603be 100644 --- a/moveit_planners/ompl/package.xml +++ b/moveit_planners/ompl/package.xml @@ -1,6 +1,6 @@ moveit_planners_ompl - 1.1.14 + 1.1.15 MoveIt interface to OMPL Ioan Sucan diff --git a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst index 0d81cfdc75..7c5b0017c2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst +++ b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package pilz_industrial_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- +* Fix identical subsequent time stamps generated by Pilz planner (`#3630 `_) +* Fix PILZ/LIN segfault (`#3624 `_) +* Contributors: Robert Haschke + 1.1.14 (2024-05-27) ------------------- * Don't copy attached collision objects in CommandListManager::setStartState (`#3590 `_) diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml index 15aa3d5396..4b7670f2c9 100644 --- a/moveit_planners/pilz_industrial_motion_planner/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner/package.xml @@ -1,7 +1,7 @@ pilz_industrial_motion_planner - 1.1.14 + 1.1.15 MoveIt plugin to generate industrial trajectories PTP, LIN, CIRC and sequences thereof. Alexander Gutenkunst diff --git a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp index fe078b2ef4..e6c6a99d71 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp @@ -54,13 +54,17 @@ std::vector PlanComponentsBuilder::build() void PlanComponentsBuilder::appendWithStrictTimeIncrease(robot_trajectory::RobotTrajectory& result, const robot_trajectory::RobotTrajectory& source) { - if (result.empty() || - !pilz_industrial_motion_planner::isRobotStateEqual(result.getLastWayPoint(), source.getFirstWayPoint(), - result.getGroupName(), ROBOT_STATE_EQUALITY_EPSILON)) + if (result.empty()) { result.append(source, 0.0); return; } + if (!pilz_industrial_motion_planner::isRobotStateEqual(result.getLastWayPoint(), source.getFirstWayPoint(), + result.getGroupName(), ROBOT_STATE_EQUALITY_EPSILON)) + { + result.append(source, source.getWayPointDurationFromStart(0)); + return; + } for (size_t i = 1; i < source.getWayPointCount(); ++i) { @@ -94,7 +98,8 @@ void PlanComponentsBuilder::blend(const planning_scene::PlanningSceneConstPtr& p // Append the new trajectory elements appendWithStrictTimeIncrease(*(traj_cont_.back()), *blend_response.first_trajectory); - traj_cont_.back()->append(*blend_response.blend_trajectory, 0.0); + appendWithStrictTimeIncrease(*(traj_cont_.back()), *blend_response.blend_trajectory); + // Store the last new trajectory element for future processing traj_tail_ = blend_response.second_trajectory; // first for next blending segment } 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 29e06d411f..5a50e6484a 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 @@ -61,7 +61,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( std::size_t second_intersection_index; if (!searchIntersectionPoints(req, first_intersection_index, second_intersection_index)) { - ROS_ERROR("Blend radius to large."); + ROS_ERROR("Blend radius too large."); res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN; return false; } @@ -118,6 +118,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend( // append the blend trajectory res.blend_trajectory->setRobotTrajectoryMsg(req.first_trajectory->getFirstWayPoint(), blend_joint_trajectory); + // copy the points [second_intersection_index, len] from the second trajectory for (size_t i = second_intersection_index + 1; i < req.second_trajectory->getWayPointCount(); ++i) { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index e07ce71e64..4054d669a2 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -33,6 +33,7 @@ *********************************************************************/ #include "pilz_industrial_motion_planner/trajectory_generator_lin.h" +#include #include #include @@ -74,7 +75,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin // goal given in joint space if (!req.goal_constraints.front().joint_constraints.empty()) { - info.link_name = robot_model_->getJointModelGroup(req.group_name)->getSolverInstance()->getTipFrame(); + info.link_name = getSolverTipFrame(robot_model_->getJointModelGroup(req.group_name)); if (req.goal_constraints.front().joint_constraints.size() != robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size()) diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst index 87e48bb540..94d6efad5e 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package pilz_industrial_motion_planner_testutils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- + 1.1.14 (2024-05-27) ------------------- diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml index 2ead397c50..030b59788f 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml @@ -1,7 +1,7 @@ pilz_industrial_motion_planner_testutils - 1.1.14 + 1.1.15 Helper scripts and functionality to test industrial motion generation Alexander Gutenkunst diff --git a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst index 8dbdbd1b4c..6957588054 100644 --- a/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_fake_controller_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_fake_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- + 1.1.14 (2024-05-27) ------------------- diff --git a/moveit_plugins/moveit_fake_controller_manager/package.xml b/moveit_plugins/moveit_fake_controller_manager/package.xml index 90fb46eece..950524c1f0 100644 --- a/moveit_plugins/moveit_fake_controller_manager/package.xml +++ b/moveit_plugins/moveit_fake_controller_manager/package.xml @@ -1,6 +1,6 @@ moveit_fake_controller_manager - 1.1.14 + 1.1.15 A fake controller manager plugin for MoveIt. Ioan Sucan diff --git a/moveit_plugins/moveit_plugins/CHANGELOG.rst b/moveit_plugins/moveit_plugins/CHANGELOG.rst index e344b6933e..d412a0b600 100644 --- a/moveit_plugins/moveit_plugins/CHANGELOG.rst +++ b/moveit_plugins/moveit_plugins/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- + 1.1.14 (2024-05-27) ------------------- diff --git a/moveit_plugins/moveit_plugins/package.xml b/moveit_plugins/moveit_plugins/package.xml index a8bb2850a6..a790476cad 100644 --- a/moveit_plugins/moveit_plugins/package.xml +++ b/moveit_plugins/moveit_plugins/package.xml @@ -1,6 +1,6 @@ moveit_plugins - 1.1.14 + 1.1.15 Metapackage for MoveIt plugins. Michael Ferguson diff --git a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst index 1d169c5239..50502eb968 100644 --- a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst +++ b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_ros_control_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- + 1.1.14 (2024-05-27) ------------------- diff --git a/moveit_plugins/moveit_ros_control_interface/package.xml b/moveit_plugins/moveit_ros_control_interface/package.xml index dcda987605..0e52e3e5fc 100644 --- a/moveit_plugins/moveit_ros_control_interface/package.xml +++ b/moveit_plugins/moveit_ros_control_interface/package.xml @@ -1,7 +1,7 @@ moveit_ros_control_interface - 1.1.14 + 1.1.15 ros_control controller manager interface for MoveIt Mathias Lüdtke diff --git a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst index e098ba9595..6af71d1cb3 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_simple_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- +* Fix race condition in trajectory execution (`#3620 `_) +* Contributors: Robert Haschke + 1.1.14 (2024-05-27) ------------------- * Simplify code waiting for ActionBasedController, fix timeout to 5s diff --git a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h index ca358b188b..798978a9f3 100644 --- a/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h +++ b/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h @@ -117,12 +117,6 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase { if (controller_action_client_ && !done_) return controller_action_client_->waitForResult(timeout); -#if 1 // TODO: remove when https://github.com/ros/actionlib/issues/155 is fixed - // workaround for actionlib issue: waitForResult() might return before our doneCB finished - ros::Time deadline = ros::Time::now() + ros::Duration(0.1); // limit waiting to 0.1s - while (!done_ && ros::ok() && ros::Time::now() < deadline) // Check the done_ flag explicitly, - ros::Duration(0.0001).sleep(); // which is eventually set in finishControllerExecution -#endif return true; } diff --git a/moveit_plugins/moveit_simple_controller_manager/package.xml b/moveit_plugins/moveit_simple_controller_manager/package.xml index 77bffbe11f..08a90450c6 100644 --- a/moveit_plugins/moveit_simple_controller_manager/package.xml +++ b/moveit_plugins/moveit_simple_controller_manager/package.xml @@ -1,6 +1,6 @@ moveit_simple_controller_manager - 1.1.14 + 1.1.15 A generic, simple controller manager plugin for MoveIt. Michael Ferguson diff --git a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp index 8881d468ef..424814fa77 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/follow_joint_trajectory_controller_handle.cpp @@ -62,11 +62,11 @@ bool FollowJointTrajectoryControllerHandle::sendTrajectory(const moveit_msgs::Ro control_msgs::FollowJointTrajectoryGoal goal = goal_template_; goal.trajectory = trajectory.joint_trajectory; + done_ = false; + last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING; controller_action_client_->sendGoal( goal, [this](const auto& state, const auto& result) { controllerDoneCallback(state, result); }, [this] { controllerActiveCallback(); }, [this](const auto& feedback) { controllerFeedbackCallback(feedback); }); - done_ = false; - last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING; return true; } diff --git a/moveit_ros/benchmarks/CHANGELOG.rst b/moveit_ros/benchmarks/CHANGELOG.rst index da23d91391..4b29a12506 100644 --- a/moveit_ros/benchmarks/CHANGELOG.rst +++ b/moveit_ros/benchmarks/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_ros_benchmarks ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- + 1.1.14 (2024-05-27) ------------------- * Use boost::timer::progress_display if available (`#3547 `_) diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml index 7b00f04fec..ed15a50084 100644 --- a/moveit_ros/benchmarks/package.xml +++ b/moveit_ros/benchmarks/package.xml @@ -1,6 +1,6 @@ moveit_ros_benchmarks - 1.1.14 + 1.1.15 Enhanced tools for benchmarks in MoveIt diff --git a/moveit_ros/manipulation/CHANGELOG.rst b/moveit_ros/manipulation/CHANGELOG.rst index 51bc6037b2..545162e9f8 100644 --- a/moveit_ros/manipulation/CHANGELOG.rst +++ b/moveit_ros/manipulation/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_manipulation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- +* New implementation for computeCartesianPath (`#3618 `_) +* Contributors: Robert Haschke + 1.1.14 (2024-05-27) ------------------- diff --git a/moveit_ros/manipulation/package.xml b/moveit_ros/manipulation/package.xml index 805fb1185f..83f02aef49 100644 --- a/moveit_ros/manipulation/package.xml +++ b/moveit_ros/manipulation/package.xml @@ -1,6 +1,6 @@ moveit_ros_manipulation - 1.1.14 + 1.1.15 Components of MoveIt used for manipulation Ioan Sucan diff --git a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h index 8c7014495f..c20d210f7c 100644 --- a/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h +++ b/moveit_ros/manipulation/pick_place/include/moveit/pick_place/approach_and_translate_stage.h @@ -58,6 +58,5 @@ class ApproachAndTranslateStage : public ManipulationStage unsigned int max_goal_count_; unsigned int max_fail_; double max_step_; - double jump_factor_; }; } // namespace pick_place diff --git a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp index ac261c4648..260897616e 100644 --- a/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp +++ b/moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp @@ -51,7 +51,6 @@ ApproachAndTranslateStage::ApproachAndTranslateStage( max_goal_count_ = GetGlobalPickPlaceParams().max_goal_count_; max_fail_ = GetGlobalPickPlaceParams().max_fail_; max_step_ = GetGlobalPickPlaceParams().max_step_; - jump_factor_ = GetGlobalPickPlaceParams().jump_factor_; } namespace @@ -250,7 +249,7 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const double d_close_up = moveit::core::CartesianInterpolator::computeCartesianPath( close_up_state.get(), plan->shared_data_->planning_group_, close_up_states, plan->shared_data_->ik_link_, approach_direction, approach_direction_is_global_frame, MAX_CLOSE_UP_DIST, - moveit::core::MaxEEFStep(max_step_), moveit::core::JumpThreshold(jump_factor_), approach_valid_callback); + moveit::core::MaxEEFStep(max_step_), moveit::core::CartesianPrecision{}, approach_valid_callback); // if progress towards the object was made, update the desired goal state if (d_close_up > 0.0 && close_up_states.size() > 1) *plan->possible_goal_states_[i] = *close_up_states[close_up_states.size() - 2]; @@ -263,8 +262,8 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const double d_approach = moveit::core::CartesianInterpolator::computeCartesianPath( first_approach_state.get(), plan->shared_data_->planning_group_, approach_states, plan->shared_data_->ik_link_, -approach_direction, approach_direction_is_global_frame, - plan->approach_.desired_distance, moveit::core::MaxEEFStep(max_step_), - moveit::core::JumpThreshold(jump_factor_), approach_valid_callback); + plan->approach_.desired_distance, moveit::core::MaxEEFStep(max_step_), moveit::core::CartesianPrecision{}, + approach_valid_callback); // if we were able to follow the approach direction for sufficient length, try to compute a retreat direction if (d_approach > plan->approach_.min_distance && !signal_stop_) @@ -298,8 +297,8 @@ bool ApproachAndTranslateStage::evaluate(const ManipulationPlanPtr& plan) const double d_retreat = moveit::core::CartesianInterpolator::computeCartesianPath( last_retreat_state.get(), plan->shared_data_->planning_group_, retreat_states, plan->shared_data_->ik_link_, retreat_direction, retreat_direction_is_global_frame, - plan->retreat_.desired_distance, moveit::core::MaxEEFStep(max_step_), - moveit::core::JumpThreshold(jump_factor_), retreat_valid_callback); + plan->retreat_.desired_distance, moveit::core::MaxEEFStep(max_step_), moveit::core::CartesianPrecision{}, + retreat_valid_callback); // if sufficient progress was made in the desired direction, we have a goal state that we can consider for // future stages diff --git a/moveit_ros/move_group/CHANGELOG.rst b/moveit_ros/move_group/CHANGELOG.rst index 9e36587349..0e5e80ffab 100644 --- a/moveit_ros/move_group/CHANGELOG.rst +++ b/moveit_ros/move_group/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_move_group ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- +* New implementation for computeCartesianPath (`#3618 `_) +* Contributors: Robert Haschke + 1.1.14 (2024-05-27) ------------------- * Gracefully handle exceptions thrown by planning pipelines (`#3481 `_) diff --git a/moveit_ros/move_group/package.xml b/moveit_ros/move_group/package.xml index b5c04e2911..a7e39f1f3d 100644 --- a/moveit_ros/move_group/package.xml +++ b/moveit_ros/move_group/package.xml @@ -1,6 +1,6 @@ moveit_ros_move_group - 1.1.14 + 1.1.15 The move_group node for MoveIt Ioan Sucan Sachin Chitta diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index c0f033f8b1..f840b7b53b 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -158,14 +158,14 @@ bool MoveGroupCartesianPathService::computeService(moveit_msgs::GetCartesianPath } ROS_INFO_NAMED(getName(), "Attempting to follow %u waypoints for link '%s' using a step of %lf m " - "and jump threshold %lf (in %s reference frame)", - (unsigned int)waypoints.size(), link_name.c_str(), req.max_step, req.jump_threshold, + "(in %s reference frame)", + (unsigned int)waypoints.size(), link_name.c_str(), req.max_step, global_frame ? "global" : "link"); std::vector traj; res.fraction = moveit::core::CartesianInterpolator::computeCartesianPath( &start_state, jmg, traj, link_model, waypoints, global_frame, moveit::core::MaxEEFStep(req.max_step), - moveit::core::JumpThreshold(req.jump_threshold), constraint_fn, kinematics::KinematicsQueryOptions(), + moveit::core::CartesianPrecision{}, constraint_fn, kinematics::KinematicsQueryOptions(), start_state.getGlobalLinkTransform(link_model).inverse() * frame_pose); moveit::core::robotStateToRobotStateMsg(start_state, res.start_state); diff --git a/moveit_ros/moveit_ros/CHANGELOG.rst b/moveit_ros/moveit_ros/CHANGELOG.rst index 3a2feea3b7..5ed1e0cdce 100644 --- a/moveit_ros/moveit_ros/CHANGELOG.rst +++ b/moveit_ros/moveit_ros/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- + 1.1.14 (2024-05-27) ------------------- diff --git a/moveit_ros/moveit_ros/package.xml b/moveit_ros/moveit_ros/package.xml index ff0a447a24..40490c18fd 100644 --- a/moveit_ros/moveit_ros/package.xml +++ b/moveit_ros/moveit_ros/package.xml @@ -1,6 +1,6 @@ moveit_ros - 1.1.14 + 1.1.15 Components of MoveIt that use ROS Ioan Sucan Sachin Chitta diff --git a/moveit_ros/moveit_servo/CHANGELOG.rst b/moveit_ros/moveit_servo/CHANGELOG.rst index 720a6c3b6b..5a81ea9f8c 100644 --- a/moveit_ros/moveit_servo/CHANGELOG.rst +++ b/moveit_ros/moveit_servo/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_servo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- +* Init joint trajectory with actual robot pose (`#3642 `_) +* Use ACM consistently (`#3641 `_) +* Contributors: Davide Torielli, Michael Görner + 1.1.14 (2024-05-27) ------------------- diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h index 08aadaad46..7fc9178a2f 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h @@ -95,9 +95,8 @@ class CollisionCheck // Pointer to the collision environment planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - // Robot state and collision matrix from planning scene - std::shared_ptr current_state_; - collision_detection::AllowedCollisionMatrix acm_; + // RobotState buffer used for collision checking + robot_state::RobotStatePtr current_state_; // Scale robot velocity according to collision proximity and user-defined thresholds. // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold. diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index 7046970e66..57f7805a7d 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -1,7 +1,7 @@ moveit_servo - 1.1.14 + 1.1.15 Provides real-time manipulator Cartesian and joint servoing. diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp index 85aa7d4810..2af18d9d73 100644 --- a/moveit_ros/moveit_servo/src/collision_check.cpp +++ b/moveit_ros/moveit_servo/src/collision_check.cpp @@ -78,8 +78,8 @@ CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_servo::ServoPar worst_case_stop_time_sub_ = internal_nh.subscribe("worst_case_stop_time", ROS_QUEUE_SIZE, &CollisionCheck::worstCaseStopTimeCB, this); + // initialize current state buffer current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); - acm_ = getLockedPlanningSceneRO()->getAllowedCollisionMatrix(); } planning_scene_monitor::LockedPlanningSceneRO CollisionCheck::getLockedPlanningSceneRO() const @@ -107,26 +107,29 @@ void CollisionCheck::run(const ros::TimerEvent& timer_event) return; } - // Update to the latest current state - current_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + // Get the latest current state (bypassing scene update throttling) + planning_scene_monitor_->getStateMonitor()->setToCurrentState(*current_state_); current_state_->updateCollisionBodyTransforms(); collision_detected_ = false; - // Do a timer-safe distance-based collision detection - collision_result_.clear(); - getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision(collision_request_, collision_result_, - *current_state_); - scene_collision_distance_ = collision_result_.distance; - collision_detected_ |= collision_result_.collision; - collision_result_.print(); - - collision_result_.clear(); - // Self-collisions and scene collisions are checked separately so different thresholds can be used - getLockedPlanningSceneRO()->getCollisionEnvUnpadded()->checkSelfCollision(collision_request_, collision_result_, - *current_state_, acm_); - self_collision_distance_ = collision_result_.distance; - collision_detected_ |= collision_result_.collision; - collision_result_.print(); + { + auto scene = getLockedPlanningSceneRO(); + auto& acm = scene->getAllowedCollisionMatrix(); + + // Do a timer-safe distance-based collision detection + collision_result_.clear(); + scene->getCollisionEnv()->checkRobotCollision(collision_request_, collision_result_, *current_state_, acm); + scene_collision_distance_ = collision_result_.distance; + collision_detected_ |= collision_result_.collision; + collision_result_.print(); + + // Self-collisions and scene collisions are checked separately so different thresholds can be used + collision_result_.clear(); + scene->getCollisionEnvUnpadded()->checkSelfCollision(collision_request_, collision_result_, *current_state_, acm); + self_collision_distance_ = collision_result_.distance; + collision_detected_ |= collision_result_.collision; + collision_result_.print(); + } velocity_scale_ = 1; // If we're definitely in collision, stop immediately diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp index 3ca03cc233..0b03130273 100644 --- a/moveit_ros/moveit_servo/src/servo_calcs.cpp +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -354,7 +354,9 @@ void ServoCalcs::calculateSingleIteration() *joint_trajectory = *last_sent_command_; for (auto& point : joint_trajectory->points) { + current_state_->copyJointGroupPositions(joint_model_group_, point.positions); point.velocities.assign(point.velocities.size(), 0); + point.accelerations.assign(point.accelerations.size(), 0); } } diff --git a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst index 818207cc16..cc5ee3821a 100644 --- a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst +++ b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_ros_occupancy_map_monitor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- + 1.1.14 (2024-05-27) ------------------- diff --git a/moveit_ros/occupancy_map_monitor/package.xml b/moveit_ros/occupancy_map_monitor/package.xml index 7dde7d09ad..f86800e152 100644 --- a/moveit_ros/occupancy_map_monitor/package.xml +++ b/moveit_ros/occupancy_map_monitor/package.xml @@ -1,7 +1,7 @@ moveit_ros_occupancy_map_monitor - 1.1.14 + 1.1.15 Components of MoveIt connecting to occupancy map Ioan Sucan diff --git a/moveit_ros/perception/CHANGELOG.rst b/moveit_ros/perception/CHANGELOG.rst index 97c4767251..8597f660fd 100644 --- a/moveit_ros/perception/CHANGELOG.rst +++ b/moveit_ros/perception/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_ros_perception ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- + 1.1.14 (2024-05-27) ------------------- diff --git a/moveit_ros/perception/package.xml b/moveit_ros/perception/package.xml index f7412dbcc4..0fe3d491fb 100644 --- a/moveit_ros/perception/package.xml +++ b/moveit_ros/perception/package.xml @@ -1,6 +1,6 @@ moveit_ros_perception - 1.1.14 + 1.1.15 Components of MoveIt connecting to perception Ioan Sucan diff --git a/moveit_ros/planning/CHANGELOG.rst b/moveit_ros/planning/CHANGELOG.rst index 8eb39e16a2..5f80205dd4 100644 --- a/moveit_ros/planning/CHANGELOG.rst +++ b/moveit_ros/planning/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_planning ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- +* PSM: Correctly handle full planning scene message (`#3610 `_) +* Contributors: Robert Haschke + 1.1.14 (2024-05-27) ------------------- * Allow waiting for joint model group states and retrieval of group-specific timestamps (`#3580 `_) diff --git a/moveit_ros/planning/package.xml b/moveit_ros/planning/package.xml index 4b02719335..92eb19ff1e 100644 --- a/moveit_ros/planning/package.xml +++ b/moveit_ros/planning/package.xml @@ -1,7 +1,7 @@ moveit_ros_planning - 1.1.14 + 1.1.15 Planning components of MoveIt that use ROS Ioan Sucan Sachin Chitta 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 79c70d3bec..a98153bb0c 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 @@ -592,7 +592,7 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::PlanningSc } else { - result = scene_->setPlanningSceneDiffMsg(scene); + result = scene_->usePlanningSceneMsg(scene); } if (octomap_monitor_) diff --git a/moveit_ros/planning_interface/CHANGELOG.rst b/moveit_ros/planning_interface/CHANGELOG.rst index 20ab0b399e..b7beb183f7 100644 --- a/moveit_ros/planning_interface/CHANGELOG.rst +++ b/moveit_ros/planning_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_ros_planning_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- +* PSI: allow persistent connections for [get|apply]_planning_scene services (`#3628 `_) +* New implementation for computeCartesianPath (`#3618 `_) +* Contributors: Cesare Tonola, Robert Haschke + 1.1.14 (2024-05-27) ------------------- * MoveGroupInterface: allow RobotState diffs as start state (`#3592 `_) 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 6079d50dea..e54c676ba3 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 @@ -779,7 +779,14 @@ class MoveGroupInterface Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the waypoints. Return -1.0 in case of error. */ - double computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, + [[deprecated("Drop jump_threshold")]] double // + computeCartesianPath(const std::vector& waypoints, double eef_step, double /*jump_threshold*/, + moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions = true, + moveit_msgs::MoveItErrorCodes* error_code = nullptr) + { + return computeCartesianPath(waypoints, eef_step, trajectory, avoid_collisions, error_code); + } + double computeCartesianPath(const std::vector& waypoints, double eef_step, moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions = true, moveit_msgs::MoveItErrorCodes* error_code = nullptr); @@ -795,7 +802,14 @@ class MoveGroupInterface Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the waypoints. Return -1.0 in case of error. */ - double computeCartesianPath(const std::vector& waypoints, double eef_step, double jump_threshold, + [[deprecated("Drop jump_threshold")]] double // + computeCartesianPath(const std::vector& waypoints, double eef_step, double /*jump_threshold*/, + moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, + bool avoid_collisions = true, moveit_msgs::MoveItErrorCodes* error_code = nullptr) + { + return computeCartesianPath(waypoints, eef_step, trajectory, path_constraints, avoid_collisions, error_code); + } + double computeCartesianPath(const std::vector& waypoints, double eef_step, moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions = true, moveit_msgs::MoveItErrorCodes* error_code = nullptr); 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 119883b64f..e8c6b1dfe5 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 @@ -941,7 +941,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl } } - double computeCartesianPath(const std::vector& waypoints, double step, double jump_threshold, + double computeCartesianPath(const std::vector& waypoints, double step, moveit_msgs::RobotTrajectory& msg, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions, moveit_msgs::MoveItErrorCodes& error_code) { @@ -954,7 +954,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl req.header.stamp = ros::Time::now(); req.waypoints = waypoints; req.max_step = step; - req.jump_threshold = jump_threshold; req.path_constraints = path_constraints; req.avoid_collisions = avoid_collisions; req.link_name = getEndEffectorLink(); @@ -1595,22 +1594,20 @@ moveit::core::MoveItErrorCode MoveGroupInterface::place(const moveit_msgs::Place } double MoveGroupInterface::computeCartesianPath(const std::vector& waypoints, double eef_step, - double jump_threshold, moveit_msgs::RobotTrajectory& trajectory, - bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code) + moveit_msgs::RobotTrajectory& trajectory, bool avoid_collisions, + moveit_msgs::MoveItErrorCodes* error_code) { - return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, moveit_msgs::Constraints(), - avoid_collisions, error_code); + return computeCartesianPath(waypoints, eef_step, trajectory, moveit_msgs::Constraints(), avoid_collisions, error_code); } double MoveGroupInterface::computeCartesianPath(const std::vector& waypoints, double eef_step, - double jump_threshold, moveit_msgs::RobotTrajectory& trajectory, + moveit_msgs::RobotTrajectory& trajectory, const moveit_msgs::Constraints& path_constraints, bool avoid_collisions, moveit_msgs::MoveItErrorCodes* error_code) { moveit_msgs::MoveItErrorCodes err_tmp; moveit_msgs::MoveItErrorCodes& err = error_code ? *error_code : err_tmp; - return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints, - avoid_collisions, err); + return impl_->computeCartesianPath(waypoints, eef_step, trajectory, path_constraints, avoid_collisions, err); } void MoveGroupInterface::stop() diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp index d3c55acd39..8918709257 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp @@ -493,24 +493,22 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer return py_bindings_tools::serializeMsg(request); } - bp::tuple computeCartesianPathPython(const bp::list& waypoints, double eef_step, double jump_threshold, - bool avoid_collisions) + bp::tuple computeCartesianPathPython(const bp::list& waypoints, double eef_step, bool avoid_collisions) { moveit_msgs::Constraints path_constraints_tmp; - return doComputeCartesianPathPython(waypoints, eef_step, jump_threshold, avoid_collisions, path_constraints_tmp); + return doComputeCartesianPathPython(waypoints, eef_step, avoid_collisions, path_constraints_tmp); } - bp::tuple computeCartesianPathConstrainedPython(const bp::list& waypoints, double eef_step, double jump_threshold, - bool avoid_collisions, + bp::tuple computeCartesianPathConstrainedPython(const bp::list& waypoints, double eef_step, bool avoid_collisions, const py_bindings_tools::ByteString& path_constraints_str) { moveit_msgs::Constraints path_constraints; py_bindings_tools::deserializeMsg(path_constraints_str, path_constraints); - return doComputeCartesianPathPython(waypoints, eef_step, jump_threshold, avoid_collisions, path_constraints); + return doComputeCartesianPathPython(waypoints, eef_step, avoid_collisions, path_constraints); } - bp::tuple doComputeCartesianPathPython(const bp::list& waypoints, double eef_step, double jump_threshold, - bool avoid_collisions, const moveit_msgs::Constraints& path_constraints) + bp::tuple doComputeCartesianPathPython(const bp::list& waypoints, double eef_step, bool avoid_collisions, + const moveit_msgs::Constraints& path_constraints) { std::vector poses; convertListToArrayOfPoses(waypoints, poses); @@ -518,7 +516,7 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer double fraction; { GILReleaser gr; - fraction = computeCartesianPath(poses, eef_step, jump_threshold, trajectory, path_constraints, avoid_collisions); + fraction = computeCartesianPath(poses, eef_step, trajectory, path_constraints, avoid_collisions); } return bp::make_tuple(py_bindings_tools::serializeMsg(trajectory), fraction); } diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index f94bddb86a..ec52cdd8f1 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -1,7 +1,7 @@ moveit_ros_planning_interface - 1.1.14 + 1.1.15 Components of MoveIt that offer simpler interfaces to planning and execution Ioan Sucan diff --git a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h index d730904f2e..ea70581eee 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h +++ b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h @@ -56,8 +56,9 @@ class PlanningSceneInterface \param ns. Namespace in which all MoveIt related topics and services are discovered \param wait. Wait for services if they are not announced in ROS. If this is false, the constructor throws std::runtime_error instead. + \param persistent_connections. Create persistent connection for the get planning scene and apply planning scene services. */ - explicit PlanningSceneInterface(const std::string& ns = "", bool wait = true); + explicit PlanningSceneInterface(const std::string& ns = "", bool wait = true, bool persistent_connections = false); ~PlanningSceneInterface(); /** diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp index 1f747c593f..e3c46ae5ba 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp @@ -51,27 +51,21 @@ static const std::string LOGNAME = "planning_scene_interface"; class PlanningSceneInterface::PlanningSceneInterfaceImpl { public: - explicit PlanningSceneInterfaceImpl(const std::string& ns = "", bool wait = true) + explicit PlanningSceneInterfaceImpl(const std::string& ns = "", bool wait = true, bool persistent_connections = false) { + wait_ = wait; + persistent_connections_ = persistent_connections; node_handle_ = ros::NodeHandle(ns); planning_scene_diff_publisher_ = node_handle_.advertise("planning_scene", 1); - planning_scene_service_ = - node_handle_.serviceClient(move_group::GET_PLANNING_SCENE_SERVICE_NAME); - apply_planning_scene_service_ = - node_handle_.serviceClient(move_group::APPLY_PLANNING_SCENE_SERVICE_NAME); - if (wait) + if (persistent_connections_ && !wait_) { - waitForService(planning_scene_service_); - waitForService(apply_planning_scene_service_); - } - else - { - if (!planning_scene_service_.exists() || !apply_planning_scene_service_.exists()) - { - throw std::runtime_error("ROS services not available"); - } + ROS_WARN_NAMED(LOGNAME, "To create persistent service clients, wait is set to true"); + wait_ = true; } + + connectGetPlanningSceneService(); + connectApplyPlanningSceneService(); } std::vector getKnownObjectNames(bool with_type) @@ -80,7 +74,8 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl moveit_msgs::GetPlanningScene::Response response; std::vector result; request.components.components = request.components.WORLD_OBJECT_NAMES; - if (!planning_scene_service_.call(request, response)) + + if (!getPlanningSceneServiceCall(request, response)) return result; if (with_type) { @@ -103,7 +98,8 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl moveit_msgs::GetPlanningScene::Response response; std::vector result; request.components.components = request.components.WORLD_OBJECT_GEOMETRY; - if (!planning_scene_service_.call(request, response)) + + if (!getPlanningSceneServiceCall(request, response)) { ROS_WARN_NAMED(LOGNAME, "Could not call planning scene service to get object names"); return result; @@ -165,7 +161,8 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl moveit_msgs::GetPlanningScene::Response response; std::map result; request.components.components = request.components.WORLD_OBJECT_GEOMETRY; - if (!planning_scene_service_.call(request, response)) + + if (!getPlanningSceneServiceCall(request, response)) { ROS_WARN_NAMED(LOGNAME, "Could not call planning scene service to get object names"); return result; @@ -187,7 +184,8 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl moveit_msgs::GetPlanningScene::Response response; std::map result; request.components.components = request.components.WORLD_OBJECT_GEOMETRY; - if (!planning_scene_service_.call(request, response)) + + if (!getPlanningSceneServiceCall(request, response)) { ROS_WARN_NAMED(LOGNAME, "Could not call planning scene service to get object geometries"); return result; @@ -210,7 +208,8 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl moveit_msgs::GetPlanningScene::Response response; std::map result; request.components.components = request.components.ROBOT_STATE_ATTACHED_OBJECTS; - if (!planning_scene_service_.call(request, response)) + + if (!getPlanningSceneServiceCall(request, response)) { ROS_WARN_NAMED(LOGNAME, "Could not call planning scene service to get attached object geometries"); return result; @@ -233,7 +232,8 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl moveit_msgs::GetPlanningScene::Request request; moveit_msgs::GetPlanningScene::Response response; request.components.components = components; - if (!planning_scene_service_.call(request, response)) + + if (!getPlanningSceneServiceCall(request, response)) { ROS_WARN_NAMED(LOGNAME, "Could not call planning scene service"); return moveit_msgs::PlanningScene(); @@ -246,7 +246,8 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl moveit_msgs::ApplyPlanningScene::Request request; moveit_msgs::ApplyPlanningScene::Response response; request.scene = planning_scene; - if (!apply_planning_scene_service_.call(request, response)) + + if (!applyPlanningSceneServiceCall(request, response)) { ROS_WARN_NAMED(LOGNAME, "Failed to call ApplyPlanningScene service"); return false; @@ -312,6 +313,73 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl } } + void connectGetPlanningSceneService() + { + if (persistent_connections_) + { + ros::service::waitForService(move_group::GET_PLANNING_SCENE_SERVICE_NAME); + } + + planning_scene_service_ = node_handle_.serviceClient( + move_group::GET_PLANNING_SCENE_SERVICE_NAME, persistent_connections_); + + if (wait_) + { + waitForService(planning_scene_service_); + } + else + { + if (!planning_scene_service_.exists()) + { + throw std::runtime_error("ROS service not available"); + } + } + } + + void connectApplyPlanningSceneService() + { + if (persistent_connections_) + { + ros::service::waitForService(move_group::APPLY_PLANNING_SCENE_SERVICE_NAME); + } + + apply_planning_scene_service_ = node_handle_.serviceClient( + move_group::APPLY_PLANNING_SCENE_SERVICE_NAME, persistent_connections_); + + if (wait_) + { + waitForService(apply_planning_scene_service_); + } + else + { + if (!apply_planning_scene_service_.exists()) + { + throw std::runtime_error("ROS service not available"); + } + } + } + + bool getPlanningSceneServiceCall(const moveit_msgs::GetPlanningScene::Request& request, + moveit_msgs::GetPlanningScene::Response& response) + { + if (!planning_scene_service_.isValid()) + connectGetPlanningSceneService(); + + return planning_scene_service_.call(request, response); + } + + bool applyPlanningSceneServiceCall(const moveit_msgs::ApplyPlanningScene::Request& request, + moveit_msgs::ApplyPlanningScene::Response& response) + { + if (!apply_planning_scene_service_.isValid()) + connectApplyPlanningSceneService(); + + return apply_planning_scene_service_.call(request, response); + } + + bool wait_; + bool persistent_connections_; + ros::NodeHandle node_handle_; ros::ServiceClient planning_scene_service_; ros::ServiceClient apply_planning_scene_service_; @@ -319,9 +387,9 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl moveit::core::RobotModelConstPtr robot_model_; }; -PlanningSceneInterface::PlanningSceneInterface(const std::string& ns, bool wait) +PlanningSceneInterface::PlanningSceneInterface(const std::string& ns, bool wait, bool persistent_connections) { - impl_ = new PlanningSceneInterfaceImpl(ns, wait); + impl_ = new PlanningSceneInterfaceImpl(ns, wait, persistent_connections); } PlanningSceneInterface::~PlanningSceneInterface() diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp index 00ee97f18e..d5b75eb77d 100644 --- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp @@ -238,6 +238,10 @@ TEST_F(MoveGroupTestFixture, PathConstraintCollisionTest) // clear path constraints move_group_->clearPathConstraints(); + + // move back to ready pose + move_group_->setNamedTarget("ready"); + planAndMove(); } TEST_F(MoveGroupTestFixture, ModifyPlanningSceneAsyncInterfaces) @@ -311,11 +315,10 @@ TEST_F(MoveGroupTestFixture, CartPathTest) waypoints.push_back(target_waypoint); // up and left moveit_msgs::RobotTrajectory trajectory; - const auto jump_threshold = 0.0; const auto eef_step = 0.01; // test below is meaningless if Cartesian planning did not succeed - ASSERT_GE(EPSILON + move_group_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory), 1.0); + ASSERT_GE(EPSILON + move_group_->computeCartesianPath(waypoints, eef_step, trajectory), 1.0); // Execute trajectory EXPECT_EQ(move_group_->execute(trajectory), moveit::core::MoveItErrorCode::SUCCESS); diff --git a/moveit_ros/planning_interface/test/subframes_test.cpp b/moveit_ros/planning_interface/test/subframes_test.cpp index 6f5bf3e1de..54a2b1f3a3 100644 --- a/moveit_ros/planning_interface/test/subframes_test.cpp +++ b/moveit_ros/planning_interface/test/subframes_test.cpp @@ -103,7 +103,7 @@ bool moveCartesianPath(const geometry_msgs::PoseStamped& pose, moveit::planning_ std::vector waypoints; waypoints.push_back(pose.pose); moveit_msgs::RobotTrajectory trajectory; - double percent = group.computeCartesianPath(waypoints, 0.01, 0, trajectory, true); + double percent = group.computeCartesianPath(waypoints, 0.01, trajectory, true); if (percent == 1.0) { group.execute(trajectory); diff --git a/moveit_ros/robot_interaction/CHANGELOG.rst b/moveit_ros/robot_interaction/CHANGELOG.rst index cde09aad15..f9c435618e 100644 --- a/moveit_ros/robot_interaction/CHANGELOG.rst +++ b/moveit_ros/robot_interaction/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_ros_robot_interaction ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- +* RobotInteraction: Don't crash on unknown eef_group +* Contributors: Robert Haschke + 1.1.14 (2024-05-27) ------------------- * Consider distance field padding for spheres (`#3506 `_) diff --git a/moveit_ros/robot_interaction/package.xml b/moveit_ros/robot_interaction/package.xml index b25dab67f6..70091c9089 100644 --- a/moveit_ros/robot_interaction/package.xml +++ b/moveit_ros/robot_interaction/package.xml @@ -1,6 +1,6 @@ moveit_ros_robot_interaction - 1.1.14 + 1.1.15 Components of MoveIt that offer interaction via interactive markers Ioan Sucan diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index c8fd7d1b2f..76a35aa086 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -402,9 +402,10 @@ void RobotInteraction::addEndEffectorMarkers(const InteractionHandlerPtr& handle marker_color.a = color[3]; moveit::core::RobotStateConstPtr rstate = handler->getState(); - const std::vector& link_names = rstate->getJointModelGroup(eef.eef_group)->getLinkModelNames(); visualization_msgs::MarkerArray marker_array; - rstate->getRobotMarkers(marker_array, link_names, marker_color, eef.eef_group, ros::Duration()); + auto jmg = rstate->getJointModelGroup(eef.eef_group); + if (jmg) + rstate->getRobotMarkers(marker_array, jmg->getLinkModelNames(), marker_color, eef.eef_group, ros::Duration()); tf2::Transform tf_root_to_link; tf2::fromMsg(tf2::toMsg(rstate->getGlobalLinkTransform(eef.parent_link)), tf_root_to_link); // Release the ptr count on the kinematic state diff --git a/moveit_ros/visualization/CHANGELOG.rst b/moveit_ros/visualization/CHANGELOG.rst index 6e20d83405..d486624abb 100644 --- a/moveit_ros/visualization/CHANGELOG.rst +++ b/moveit_ros/visualization/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package moveit_ros_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- +* New implementation for computeCartesianPath (`#3618 `_) +* rviz joints tab: show goal state model onFinishedExecution() +* nullspace exploration: reset sliders to zero before hiding (`#3613 `_) +* Contributors: Robert Haschke + 1.1.14 (2024-05-27) ------------------- * Radian/Degree joint value edits in RViz (`#3542 `_) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp index 73995c9e9d..58fe3cf9b5 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_joints_widget.cpp @@ -300,6 +300,8 @@ void MotionPlanningFrameJointsWidget::queryGoalStateChanged() void MotionPlanningFrameJointsWidget::setActiveModel(JMGItemModel* model) { + if (ui_->joints_view_->model() == model) + return; ui_->joints_view_->setModel(model); ui_->joints_view_label_->setText( QString("Group joints of %1 state").arg(model == start_state_model_.get() ? "start" : "goal")); @@ -388,7 +390,10 @@ void MotionPlanningFrameJointsWidget::updateNullspaceSliders() // hide remaining sliders for (; i < ns_sliders_.size(); ++i) + { + ns_sliders_[i]->setValue(0); ns_sliders_[i]->hide(); + } } QSlider* MotionPlanningFrameJointsWidget::createNSSlider(int i) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 4f18a2fef5..4a75865f66 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -129,13 +129,11 @@ bool MotionPlanningFrame::computeCartesianPlan() // setup default params double cart_step_size = 0.01; - double cart_jump_thresh = 0.0; bool avoid_collisions = true; // compute trajectory moveit_msgs::RobotTrajectory trajectory; - double fraction = - move_group_->computeCartesianPath(waypoints, cart_step_size, cart_jump_thresh, trajectory, avoid_collisions); + double fraction = move_group_->computeCartesianPath(waypoints, cart_step_size, trajectory, avoid_collisions); if (fraction >= 1.0) { @@ -253,10 +251,9 @@ void MotionPlanningFrame::onFinishedExecution(bool success) if (ui_->start_state_combo_box->currentText() == "") startStateTextChanged(ui_->start_state_combo_box->currentText()); - // auto-update goal to stored previous state (but only on success) - // on failure, the user must update the goal to the previous state himself - if (ui_->goal_state_combo_box->currentText() == "") - goalStateTextChanged(ui_->goal_state_combo_box->currentText()); + // update query goal state (from previous or to current) + // also ensures that joints tab shows goal state model + goalStateTextChanged(ui_->goal_state_combo_box->currentText()); } void MotionPlanningFrame::onNewPlanningSceneState() diff --git a/moveit_ros/visualization/package.xml b/moveit_ros/visualization/package.xml index 8b5b1e2137..0e67396462 100644 --- a/moveit_ros/visualization/package.xml +++ b/moveit_ros/visualization/package.xml @@ -1,6 +1,6 @@ moveit_ros_visualization - 1.1.14 + 1.1.15 Components of MoveIt that offer visualization Ioan Sucan diff --git a/moveit_ros/warehouse/CHANGELOG.rst b/moveit_ros/warehouse/CHANGELOG.rst index 4d408b4b8a..b11eea331b 100644 --- a/moveit_ros/warehouse/CHANGELOG.rst +++ b/moveit_ros/warehouse/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_ros_warehouse ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- + 1.1.14 (2024-05-27) ------------------- * Fix warehouse database destruction (`#3581 `_) diff --git a/moveit_ros/warehouse/package.xml b/moveit_ros/warehouse/package.xml index beec929e4f..7ee0c13a22 100644 --- a/moveit_ros/warehouse/package.xml +++ b/moveit_ros/warehouse/package.xml @@ -1,6 +1,6 @@ moveit_ros_warehouse - 1.1.14 + 1.1.15 Components of MoveIt connecting to MongoDB Ioan Sucan diff --git a/moveit_runtime/CHANGELOG.rst b/moveit_runtime/CHANGELOG.rst index 79e847a6fd..79310301e8 100644 --- a/moveit_runtime/CHANGELOG.rst +++ b/moveit_runtime/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package moveit_runtime ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- + 1.1.14 (2024-05-27) ------------------- diff --git a/moveit_runtime/package.xml b/moveit_runtime/package.xml index 865234772f..d5a9aef4d2 100644 --- a/moveit_runtime/package.xml +++ b/moveit_runtime/package.xml @@ -1,7 +1,7 @@ moveit_runtime - 1.1.14 + 1.1.15 moveit_runtime meta package contains MoveIt packages that are essential for its runtime (e.g. running MoveIt on robots). Isaac I. Y. Saito diff --git a/moveit_setup_assistant/CHANGELOG.rst b/moveit_setup_assistant/CHANGELOG.rst index 5762476fc2..9771ad3f5a 100644 --- a/moveit_setup_assistant/CHANGELOG.rst +++ b/moveit_setup_assistant/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_setup_assistant ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.1.15 (2024-09-09) +------------------- +* Fix formatting of boost option definitions +* Contributors: Robert Haschke + 1.1.14 (2024-05-27) ------------------- * MSA: Add missing filename attribute to gazebo plugin tag (`#3572 `_) diff --git a/moveit_setup_assistant/package.xml b/moveit_setup_assistant/package.xml index 3b2bf55038..70c44f5eaa 100644 --- a/moveit_setup_assistant/package.xml +++ b/moveit_setup_assistant/package.xml @@ -1,6 +1,6 @@ moveit_setup_assistant - 1.1.14 + 1.1.15 Generates a configuration package that makes it easy to use MoveIt diff --git a/moveit_setup_assistant/src/collisions_updater.cpp b/moveit_setup_assistant/src/collisions_updater.cpp index 372671e64c..99ff4a6a99 100644 --- a/moveit_setup_assistant/src/collisions_updater.cpp +++ b/moveit_setup_assistant/src/collisions_updater.cpp @@ -179,22 +179,20 @@ int main(int argc, char* argv[]) uint32_t never_trials = 0; po::options_description desc("Allowed options"); - desc.add_options()("help", "show help")("config-pkg", po::value(&config_pkg_path), "path to MoveIt config package")( - "urdf", po::value(&urdf_path), - "path to URDF ( or xacro)")("srdf", po::value(&srdf_path), - "path to SRDF ( or xacro)")("output", po::value(&output_path), "output path for SRDF") - - ("xacro-args", po::value >()->composing(), "additional arguments for xacro") - - ("default", po::bool_switch(&include_default), "disable default colliding pairs")( - "always", po::bool_switch(&include_always), "disable always colliding pairs") - - ("keep", po::bool_switch(&keep_old), "keep disabled link from SRDF")("verbose", po::bool_switch(&verbose), - "verbose output") - - ("trials", po::value(&never_trials), "number of trials for searching never colliding pairs")( - "min-collision-fraction", po::value(&min_collision_fraction), - "fraction of small sample size to determine links that are alwas colliding"); + desc.add_options() // clang-format off + ("help", "show help") + ("config-pkg", po::value(&config_pkg_path), "path to MoveIt config package") + ("urdf", po::value(&urdf_path), "path to URDF ( or xacro)") + ("srdf", po::value(&srdf_path), "path to SRDF ( or xacro)") + ("output", po::value(&output_path), "output path for SRDF") + ("xacro-args", po::value >()->composing(), "additional arguments for xacro") + ("default", po::bool_switch(&include_default), "disable default colliding pairs") + ("always", po::bool_switch(&include_always), "disable always colliding pairs") + ("keep", po::bool_switch(&keep_old), "keep disabled link from SRDF") + ("verbose", po::bool_switch(&verbose), "verbose output") + ("trials", po::value(&never_trials), "number of trials for searching never colliding pairs") + ("min-collision-fraction", po::value(&min_collision_fraction), "fraction of small sample size to determine links that are alwas colliding") + ; // clang-format on po::positional_options_description pos_desc; pos_desc.add("xacro-args", -1);