diff --git a/.codespell_words b/.codespell_words index 33d567ff91..6be59c42cd 100644 --- a/.codespell_words +++ b/.codespell_words @@ -1,19 +1,19 @@ +aas ang ans atleast ba +brin dof dur iff keyserver nto ot +padd parameterizes planed +sinic tork uint whis -sinic -padd -brin -aas diff --git a/.docker/tutorial-source/Dockerfile b/.docker/tutorial-source/Dockerfile index e4f875e91b..933513c375 100644 --- a/.docker/tutorial-source/Dockerfile +++ b/.docker/tutorial-source/Dockerfile @@ -1,12 +1,16 @@ # syntax = docker/dockerfile:1.3 # ghcr.io/moveit/moveit2:main-${ROS_DISTRO}-tutorial-source -# Source build of the repos file from the tutorail site +# Source build of the repos file from the tutorial site ARG ROS_DISTRO=rolling +ARG GZ_VERSION=harmonic + FROM moveit/moveit2:${ROS_DISTRO}-ci LABEL maintainer Tyler Weaver tyler@picknik.ai +ARG GZ_VERSION + # Export ROS_UNDERLAY for downstream docker containers ENV ROS_UNDERLAY /root/ws_moveit/install WORKDIR $ROS_UNDERLAY/.. @@ -17,6 +21,11 @@ COPY . src/moveit2 # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size # https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#minimize-the-number-of-layers RUN --mount=type=cache,target=/root/.ccache/,sharing=locked \ + # Install Gazebo, which is needed by some dependencies. + sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' && \ + wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - && \ + sudo apt update && \ + sudo apt-get install -y "gz-${GZ_VERSION}" && \ # Enable ccache PATH=/usr/lib/ccache:$PATH && \ # Checkout the tutorial repo diff --git a/.github/workflows/sonar.yaml b/.github/workflows/sonar.yaml index 083d6c07ca..8febc84e0c 100644 --- a/.github/workflows/sonar.yaml +++ b/.github/workflows/sonar.yaml @@ -110,7 +110,7 @@ jobs: run: cat ${{ github.workspace }}/.work/target_ws/coverage.info - name: Install sonar-scanner if: steps.ici.outputs.target_test_results == '0' - uses: SonarSource/sonarcloud-github-c-cpp@v2 + uses: SonarSource/sonarcloud-github-c-cpp@v3 - name: Run sonar-scanner if: steps.ici.outputs.target_test_results == '0' env: diff --git a/.github/workflows/tutorial_docker.yaml b/.github/workflows/tutorial_docker.yaml index 2438e4750f..cd305eec20 100644 --- a/.github/workflows/tutorial_docker.yaml +++ b/.github/workflows/tutorial_docker.yaml @@ -16,7 +16,7 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling, humble, iron] + ROS_DISTRO: [rolling, humble, iron, jazzy] runs-on: ubuntu-latest permissions: packages: write diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 129be497ef..c8968d84dc 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -47,7 +47,7 @@ repos: files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: ['-fallback-style=none', '-i'] - repo: https://github.com/codespell-project/codespell - rev: v2.2.6 + rev: v2.3.0 hooks: - id: codespell args: ['--write-changes', '--ignore-words=.codespell_words', '--skip="*.eps"'] diff --git a/README.md b/README.md index fa3d75cc09..9504b3e69c 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -MoveIt Logo +MoveIt Logo The [MoveIt Motion Planning Framework for ROS 2](http://moveit.ros.org). For the ROS 1 repository see [MoveIt 1](https://github.com/moveit/moveit). For the commercially supported version see [MoveIt Pro](http://picknik.ai/pro). diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h index 92163406aa..061a304085 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -155,11 +155,24 @@ TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_1) ASSERT_TRUE(res.collision); res.clear(); + pos1.translation().z() = 0.25; + this->cenv_->getWorld()->moveObject("box", pos1); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + pos1.translation().z() = 0.05; this->cenv_->getWorld()->moveObject("box", pos1); this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); ASSERT_TRUE(res.collision); res.clear(); + pos1.translation().z() = 0.25; + this->cenv_->getWorld()->moveObject("box", pos1); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + res.clear(); + this->cenv_->getWorld()->moveObject("box", pos1); ASSERT_FALSE(res.collision); } diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/world.h b/moveit_core/collision_detection/include/moveit/collision_detection/world.h index 92799537c5..d610d4b43c 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/world.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/world.h @@ -217,6 +217,9 @@ class World bool moveShapeInObject(const std::string& object_id, const shapes::ShapeConstPtr& shape, const Eigen::Isometry3d& shape_pose); + /** \brief Update the pose of all shapes in an object. Shape size is verified. Returns true on success. */ + bool moveShapesInObject(const std::string& object_id, const EigenSTL::vector_Isometry3d& shape_poses); + /** \brief Move the object pose (thus moving all shapes and subframes in the object) * according to the given transform specified in world frame. * The transform is relative to and changes the object pose. It does not replace it. diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp index eae07cf01c..ce7c6006fc 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -259,6 +259,26 @@ bool World::moveShapeInObject(const std::string& object_id, const shapes::ShapeC return false; } +bool World::moveShapesInObject(const std::string& object_id, const EigenSTL::vector_Isometry3d& shape_poses) +{ + auto it = objects_.find(object_id); + if (it != objects_.end()) + { + if (shape_poses.size() == it->second->shapes_.size()) + { + for (std::size_t i = 0; i < shape_poses.size(); ++i) + { + ASSERT_ISOMETRY(shape_poses[i]) // unsanitized input, could contain a non-isometry + it->second->shape_poses_[i] = shape_poses[i]; + it->second->global_shape_poses_[i] = it->second->pose_ * shape_poses[i]; + } + notify(it->second, MOVE_SHAPE); + return true; + } + } + return false; +} + bool World::moveObject(const std::string& object_id, const Eigen::Isometry3d& transform) { const auto it = objects_.find(object_id); diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp index 54a007d325..86a48b8320 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -450,6 +450,38 @@ void CollisionEnvFCL::notifyObjectChange(const ObjectConstPtr& obj, World::Actio } cleanCollisionGeometryCache(); } + else if (action == World::MOVE_SHAPE) + { + auto it = fcl_objs_.find(obj->id_); + if (it == fcl_objs_.end()) + { + RCLCPP_ERROR(getLogger(), "Cannot move shapes of unknown FCL object: '%s'", obj->id_.c_str()); + return; + } + + if (obj->global_shape_poses_.size() != it->second.collision_objects_.size()) + { + RCLCPP_ERROR(getLogger(), + "Cannot move shapes, shape size mismatch between FCL object and world object: '%s'. Respectively " + "%zu and %zu.", + obj->id_.c_str(), it->second.collision_objects_.size(), it->second.collision_objects_.size()); + return; + } + + for (std::size_t i = 0; i < it->second.collision_objects_.size(); ++i) + { + it->second.collision_objects_[i]->setTransform(transform2fcl(obj->global_shape_poses_[i])); + + // compute AABB, order matters + it->second.collision_geometry_[i]->collision_geometry_->computeLocalAABB(); + it->second.collision_objects_[i]->computeAABB(); + } + + // update AABB in the FCL broadphase manager tree + // see https://github.com/moveit/moveit/pull/3601 for benchmarks + it->second.unregisterFrom(manager_.get()); + it->second.registerTo(manager_.get()); + } else { updateFCLObject(obj->id_); diff --git a/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h b/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h index f03a42d39f..5af220a81e 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h +++ b/moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h @@ -48,7 +48,7 @@ namespace distance_field * * @param [in] body The body to discretize * @param [in] resolution The resolution at which to test - * @param [out] points The points internal to the body are appended to thiss + * @param [out] points The points internal to the body are appended to this * vector. */ void findInternalPointsConvex(const bodies::Body& body, double resolution, EigenSTL::vector_Vector3d& points); diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h index 80212d2fd2..8f5e5b362f 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h @@ -60,7 +60,7 @@ namespace online_signal_smoothing * It comes from finding the bilinear transform equivalent of the analog transfer function and * further applying the inverse z-transform. * The parameter "low_pass_filter_coeff" equals (2*pi / tan(omega_d * T)) - * where omega_d is the cutoff frequency and T is the samping period in sec. + * where omega_d is the cutoff frequency and T is the sampling period in sec. */ class ButterworthFilter { @@ -69,7 +69,7 @@ class ButterworthFilter * Constructor. * @param low_pass_filter_coeff Larger filter_coeff-> more smoothing of commands, but more lag. * low_pass_filter_coeff = (2*pi / tan(omega_d * T)) - * where omega_d is the cutoff frequency and T is the samping period in sec. + * where omega_d is the cutoff frequency and T is the sampling period in sec. */ ButterworthFilter(double low_pass_filter_coeff); ButterworthFilter() = delete; diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index eaffc31d42..8655117fb8 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -210,10 +210,14 @@ PlanningScene::PlanningScene(const PlanningSceneConstPtr& parent) : parent_(pare robot_model_ = parent_->robot_model_; + setStateFeasibilityPredicate(parent->getStateFeasibilityPredicate()); + setMotionFeasibilityPredicate(parent->getMotionFeasibilityPredicate()); + // maintain a separate world. Copy on write ensures that most of the object // info is shared until it is modified. world_ = std::make_shared(*parent_->world_); world_const_ = world_; + setCollisionObjectUpdateCallback(parent_->current_world_object_update_callback_); // record changes to the world world_diff_ = std::make_shared(world_); @@ -650,7 +654,14 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& sce { if (it.first == OCTOMAP_NS) { - do_omap = true; + if (it.second == collision_detection::World::DESTROY) + { + scene_msg.world.octomap.octomap.id = "cleared"; // indicate cleared octomap + } + else + { + do_omap = true; + } } else if (it.second == collision_detection::World::DESTROY) { @@ -1203,7 +1214,6 @@ void PlanningScene::decoupleParent() (object_types_.value())[it->first] = it->second; } } - parent_.reset(); } @@ -1254,7 +1264,7 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScen result &= processCollisionObjectMsg(collision_object); // if an octomap was specified, replace the one we have with that one - if (!scene_msg.world.octomap.octomap.data.empty()) + if (!scene_msg.world.octomap.octomap.id.empty()) processOctomapMsg(scene_msg.world.octomap); return result; @@ -1717,16 +1727,16 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs:: shapes.reserve(num_shapes); shape_poses.reserve(num_shapes); - utilities::poseMsgToEigen(object.pose, object_pose); - bool switch_object_pose_and_shape_pose = false; - if (num_shapes == 1) + if (num_shapes == 1 && moveit::core::isEmpty(object.pose)) { - if (moveit::core::isEmpty(object.pose)) - { - switch_object_pose_and_shape_pose = true; // If the object pose is not set but the shape pose is, - // use the shape's pose as the object pose. - } + // If the object pose is not set but the shape pose is, use the shape's pose as the object pose. + switch_object_pose_and_shape_pose = true; + object_pose.setIdentity(); + } + else + { + utilities::poseMsgToEigen(object.pose, object_pose); } auto append = [&object_pose, &shapes, &shape_poses, @@ -1848,6 +1858,7 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision { if (world_->hasObject(object.id)) { + // update object pose if (!object.primitives.empty() || !object.meshes.empty() || !object.planes.empty()) { RCLCPP_WARN(getLogger(), "Move operation for object '%s' ignores the geometry specified in the message.", @@ -1861,6 +1872,47 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision const Eigen::Isometry3d object_frame_transform = world_to_object_header_transform * header_to_pose_transform; world_->setObjectPose(object.id, object_frame_transform); + + // update shape poses + if (!object.primitive_poses.empty() || !object.mesh_poses.empty() || !object.plane_poses.empty()) + { + auto world_object = world_->getObject(object.id); // object exists, checked earlier + + std::size_t shape_size = object.primitive_poses.size() + object.mesh_poses.size() + object.plane_poses.size(); + if (shape_size != world_object->shape_poses_.size()) + { + RCLCPP_ERROR(getLogger(), + "Move operation for object '%s' must have same number of geometry poses. Cannot move.", + object.id.c_str()); + return false; + } + + // order matters -> primitive, mesh and plane + EigenSTL::vector_Isometry3d shape_poses; + for (const auto& shape_pose : object.primitive_poses) + { + shape_poses.emplace_back(); + utilities::poseMsgToEigen(shape_pose, shape_poses.back()); + } + for (const auto& shape_pose : object.mesh_poses) + { + shape_poses.emplace_back(); + utilities::poseMsgToEigen(shape_pose, shape_poses.back()); + } + for (const auto& shape_pose : object.plane_poses) + { + shape_poses.emplace_back(); + utilities::poseMsgToEigen(shape_pose, shape_poses.back()); + } + + if (!world_->moveShapesInObject(object.id, shape_poses)) + { + RCLCPP_ERROR(getLogger(), "Move operation for object '%s' internal world error. Cannot move.", + object.id.c_str()); + return false; + } + } + return true; } diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index 4bd5c25ce9..909f3f6576 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -44,6 +44,8 @@ #include #include #include +#include +#include #include #include @@ -89,6 +91,59 @@ TEST(PlanningScene, LoadRestore) EXPECT_EQ(ps.getRobotModel()->getName(), ps_msg.robot_model_name); } +TEST(PlanningScene, LoadOctomap) +{ + urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2"); + srdf::ModelSharedPtr srdf_model(new srdf::Model()); + planning_scene::PlanningScene ps(urdf_model, srdf_model); + + { // check octomap before doing any operations on it + octomap_msgs::msg::OctomapWithPose msg; + ps.getOctomapMsg(msg); + EXPECT_TRUE(msg.octomap.id.empty()); + EXPECT_TRUE(msg.octomap.data.empty()); + } + + { // fill PlanningScene's octomap + octomap::OcTree octomap(0.1); + octomap::point3d origin(0, 0, 0); + octomap::point3d end(0, 1, 2); + octomap.insertRay(origin, end); + + // populate PlanningScene with octomap + moveit_msgs::msg::PlanningScene msg; + msg.is_diff = true; + octomap_msgs::fullMapToMsg(octomap, msg.world.octomap.octomap); + ps.setPlanningSceneDiffMsg(msg); + + // validate octomap message + octomap_msgs::msg::OctomapWithPose octomap_msg; + ps.getOctomapMsg(octomap_msg); + EXPECT_EQ(octomap_msg.octomap.id, "OcTree"); + EXPECT_EQ(octomap_msg.octomap.data.size(), msg.world.octomap.octomap.data.size()); + } + + { // verify that a PlanningScene msg with an empty octomap id does not modify the octomap + // create planning scene + moveit_msgs::msg::PlanningScene msg; + msg.is_diff = true; + ps.setPlanningSceneDiffMsg(msg); + + octomap_msgs::msg::OctomapWithPose octomap_msg; + ps.getOctomapMsg(octomap_msg); + EXPECT_EQ(octomap_msg.octomap.id, "OcTree"); + EXPECT_FALSE(octomap_msg.octomap.data.empty()); + } + + { // check that a non-empty octomap id, but empty octomap will clear the octomap + moveit_msgs::msg::PlanningScene msg; + msg.is_diff = true; + msg.world.octomap.octomap.id = "xxx"; + ps.setPlanningSceneDiffMsg(msg); + EXPECT_FALSE(static_cast(ps.getWorld()->getObject(planning_scene::PlanningScene::OCTOMAP_NS))); + } +} + TEST(PlanningScene, LoadRestoreDiff) { urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2"); diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index 1f77946234..ed2887e2a2 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -244,6 +244,17 @@ class RobotTrajectory void swap(robot_trajectory::RobotTrajectory& other) noexcept; + /** + * \brief Remove a point from the trajectory + * \param index - the index to remove + */ + RobotTrajectory& removeWayPoint(std::size_t index) + { + waypoints_.erase(waypoints_.begin() + index); + duration_from_previous_.erase(duration_from_previous_.begin() + index); + return *this; + } + RobotTrajectory& clear() { waypoints_.clear(); diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit.test b/moveit_planners/chomp/chomp_interface/test/chomp_moveit.test deleted file mode 100644 index e965ff64b3..0000000000 --- a/moveit_planners/chomp/chomp_interface/test/chomp_moveit.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_panda.test b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_panda.test new file mode 100644 index 0000000000..952f150350 --- /dev/null +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_panda.test @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_rrbot.test b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_rrbot.test new file mode 100644 index 0000000000..47c4dfe61b --- /dev/null +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_rrbot.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_panda.cpp b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_panda.cpp new file mode 100644 index 0000000000..0d84b43521 --- /dev/null +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_panda.cpp @@ -0,0 +1,80 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Sherbrooke University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/// \author Captain Yoshi + +#include +#include + +#include +#include + +class CHOMPMoveitTest : public ::testing::Test +{ +public: + moveit::planning_interface::MoveGroupInterface move_group_; + moveit::planning_interface::MoveGroupInterface::Plan my_plan_; + +public: + CHOMPMoveitTest() : move_group_("hand") + { + } +}; + +// TEST CASES + +// https://github.com/moveit/moveit/issues/2542 +TEST_F(CHOMPMoveitTest, jointSpaceGoodGoal) +{ + move_group_.setStartState(*(move_group_.getCurrentState())); + move_group_.setJointValueTarget(std::vector({ 0.0, 0.0 })); + move_group_.setPlanningTime(5.0); + + moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_); + EXPECT_GT(my_plan_.trajectory_.joint_trajectory.points.size(), 0u); + EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::SUCCESS); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "chomp_moveit_test_panda"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_rrbot.cpp similarity index 98% rename from moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp rename to moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_rrbot.cpp index 8e79efa70b..7e69f51c41 100644 --- a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_rrbot.cpp @@ -107,7 +107,7 @@ TEST_F(CHOMPMoveitTest, collisionAtEndOfPath) int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "chomp_moveit_test"); + ros::init(argc, argv, "chomp_moveit_test_rrbot"); ros::AsyncSpinner spinner(1); spinner.start(); diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp index d1fc2c2091..f31f48510d 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp @@ -205,26 +205,15 @@ void ChompOptimizer::initialize() { if (fixed_link_resolution_map.find(link->getParentJointModel()->getName()) == fixed_link_resolution_map.end()) { - const moveit::core::JointModel* parent_model = nullptr; - bool found_root = false; - - while (!found_root) + const moveit::core::JointModel* parent_model = link->getParentJointModel(); + while (true) // traverse up the tree until we find a joint we know about in joint_names_ { - if (parent_model == nullptr) - { - parent_model = link->getParentJointModel(); - } - else - { - parent_model = parent_model->getParentLinkModel()->getParentJointModel(); - for (const std::string& joint_name : joint_names_) - { - if (parent_model->getName() == joint_name) - { - found_root = true; - } - } - } + if (parent_model->getParentLinkModel() == nullptr) + break; + + parent_model = parent_model->getParentLinkModel()->getParentJointModel(); + if (std::find(joint_names_.begin(), joint_names_.end(), parent_model->getName()) != joint_names_.end()) + break; } fixed_link_resolution_map[link->getParentJointModel()->getName()] = parent_model->getName(); } @@ -963,7 +952,7 @@ void ChompOptimizer::setRobotStateFromPoint(ChompTrajectory& group_trajectory, i for (size_t j = 0; j < group_trajectory.getNumJoints(); ++j) joint_states.emplace_back(point(0, j)); - state_.setJointGroupPositions(planning_group_, joint_states); + state_.setJointGroupActivePositions(planning_group_, joint_states); state_.update(); } diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h index 4ebe272e8d..6f7837ae3a 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h @@ -145,6 +145,5 @@ class PoseModelStateSpace : public ModelBasedStateSpace }; std::vector poses_; - double jump_factor_; }; } // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp index 1325a13638..12d95371ab 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp @@ -54,8 +54,6 @@ const std::string PoseModelStateSpace::PARAMETERIZATION_TYPE = "PoseModel"; PoseModelStateSpace::PoseModelStateSpace(const ModelBasedStateSpaceSpecification& spec) : ModelBasedStateSpace(spec) { - jump_factor_ = 3; // \todo make this a param - if (spec.joint_model_group_->getGroupKinematics().first) { poses_.emplace_back(spec.joint_model_group_, spec.joint_model_group_->getGroupKinematics().first); @@ -82,10 +80,7 @@ PoseModelStateSpace::~PoseModelStateSpace() = default; double PoseModelStateSpace::distance(const ompl::base::State* state1, const ompl::base::State* state2) const { - double total = 0; - for (std::size_t i = 0; i < poses_.size(); ++i) - total += poses_[i].state_space_->distance(state1->as()->poses[i], state2->as()->poses[i]); - return total; + return ModelBasedStateSpace::distance(state1, state2); } double PoseModelStateSpace::getMaximumExtent() const @@ -136,42 +131,9 @@ void PoseModelStateSpace::sanityChecks() const void PoseModelStateSpace::interpolate(const ompl::base::State* from, const ompl::base::State* to, const double t, ompl::base::State* state) const { - // we want to interpolate in Cartesian space; we do not have a guarantee that from and to - // have their poses computed, but this is very unlikely to happen (depends how the planner gets its input states) - // interpolate in joint space ModelBasedStateSpace::interpolate(from, to, t, state); - - // interpolate SE3 components - for (std::size_t i = 0; i < poses_.size(); ++i) - { - poses_[i].state_space_->interpolate(from->as()->poses[i], to->as()->poses[i], t, - state->as()->poses[i]); - } - - // the call above may reset all flags for state; but we know the pose we want flag should be set - state->as()->setPoseComputed(true); - - /* - std::cout << "*********** interpolate\n"; - printState(from, std::cout); - printState(to, std::cout); - printState(state, std::cout); - std::cout << "\n\n"; - */ - - // after interpolation we cannot be sure about the joint values (we use them as seed only) - // so we recompute IK if needed - if (computeStateIK(state)) - { - double dj = jump_factor_ * ModelBasedStateSpace::distance(from, to); - double d_from = ModelBasedStateSpace::distance(from, state); - double d_to = ModelBasedStateSpace::distance(state, to); - - // if the joint value jumped too much - if (d_from + d_to > std::max(0.2, dj)) // \todo make 0.2 a param - state->as()->markInvalid(); - } + computeStateFK(state); } void PoseModelStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h index 4835dc8876..e810ae4726 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h @@ -38,7 +38,6 @@ #include #include -#include namespace pilz_industrial_motion_planner { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp index be11aa84da..de829de34a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp @@ -112,7 +112,26 @@ RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConst // therefore: "i-1". (i > 0 ? radii.at(i - 1) : 0.)); } - return plan_comp_builder_.build(); + + const auto res_vec = plan_comp_builder_.build(); + + // De-duplicate trajectory points with the same time value. + // This is necessary since some controllers do not allow times that are not monotonically increasing. + // TODO: Ideally, we would not need this code if the trajectory segments were created without + // duplicate time points in the first place. Leaving this note to revisit this with a more principled fix. + for (const auto& traj : res_vec) + { + for (size_t i = 0; i < traj->size() - 1; ++i) + { + if (traj->getWayPointDurationFromStart(i) == traj->getWayPointDurationFromStart(i + 1)) + { + RCLCPP_WARN(getLogger(), "Removed duplicate point at time=%f", traj->getWayPointDurationFromStart(i)); + traj->removeWayPoint(i + 1); + } + } + } + + return res_vec; } bool CommandListManager::checkRadiiForOverlap(const robot_trajectory::RobotTrajectory& traj_A, const double radii_A, diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp index a2998e6ecb..159640da14 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp @@ -274,7 +274,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::searchIn // (last point of the first trajectory, first point of the second trajectory) Eigen::Isometry3d circ_pose = req.first_trajectory->getLastWayPoint().getFrameTransform(req.link_name); - // Searh for intersection points according to distance + // Search for intersection points according to distance if (!linearSearchIntersectionPoint(req.link_name, circ_pose.translation(), req.blend_radius, req.first_trajectory, true, first_interse_index)) { diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp index 5997f1cadf..40a037675a 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp @@ -171,7 +171,7 @@ void initMoveitPy(py::module& m) Returns the planning scene monitor. )") - .def("get_trajactory_execution_manager", &moveit_cpp::MoveItCpp::getTrajectoryExecutionManagerNonConst, + .def("get_trajectory_execution_manager", &moveit_cpp::MoveItCpp::getTrajectoryExecutionManagerNonConst, py::return_value_policy::reference, R"( Returns the trajectory execution manager. diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp index 6101ab60cc..c2cd031305 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp @@ -223,10 +223,10 @@ bool MoveGroupKinematicsService::computeFKService(const std::shared_ptrrobot_state, rs); for (std::size_t i = 0; i < req->fk_link_names.size(); ++i) { - if (rs.getRobotModel()->hasLinkModel(req->fk_link_names[i])) + if (rs.knowsFrameTransform(req->fk_link_names[i])) { res->pose_stamped.resize(res->pose_stamped.size() + 1); - res->pose_stamped.back().pose = tf2::toMsg(rs.getGlobalLinkTransform(req->fk_link_names[i])); + res->pose_stamped.back().pose = tf2::toMsg(rs.getFrameTransform(req->fk_link_names[i])); res->pose_stamped.back().header.frame_id = default_frame; res->pose_stamped.back().header.stamp = context_->moveit_cpp_->getNode()->get_clock()->now(); if (do_transform) diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index b5d57bfcb9..5037999513 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml @@ -12,7 +12,7 @@ max_expected_latency: 0.1 # delay between sending a command and the robot execu command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" - linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + linear: 0.2 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. joint: 0.5 @@ -41,9 +41,9 @@ check_octomap_collisions: false # Check collision against the octomap (if a 3D move_group_name: panda_arm # Often 'manipulator' or 'arm' ## Configure handling of singularities and joint limits -lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) +lower_singularity_threshold: 10.0 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this -joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # added as a buffer to joint limits [radians or meters]. If moving quickly, make this larger. +joint_limit_margins: [0.12, 0.12, 0.12, 0.12, 0.12, 0.12, 0.12] # added as a buffer to joint limits [radians or meters]. If moving quickly, make this larger. leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/moveit/moveit2/pull/620) ## Topic names diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index 6e2b8678d8..bce0275430 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include @@ -96,7 +97,6 @@ void PointCloudOctomapUpdater::start() if (!ns_.empty()) prefix = ns_ + "/"; - rclcpp::QoS qos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data)); if (!filtered_cloud_topic_.empty()) { filtered_cloud_publisher_ = @@ -107,7 +107,11 @@ void PointCloudOctomapUpdater::start() return; /* subscribe to point cloud topic using tf filter*/ point_cloud_subscriber_ = new message_filters::Subscriber(node_, point_cloud_topic_, +#if RCLCPP_VERSION_GTE(28, 3, 0) + rclcpp::SensorDataQoS()); +#else rmw_qos_profile_sensor_data); +#endif if (tf_listener_ && tf_buffer_ && !monitor_->getMapFrame().empty()) { point_cloud_filter_ = new tf2_ros::MessageFilter( diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index f3c6edaa7b..15251ccdc9 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -57,27 +57,28 @@ class MoveItCpp /// Specification of options to use when constructing the MoveItCpp class struct PlanningSceneMonitorOptions { + PlanningSceneMonitorOptions() + : joint_state_topic(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC) + , attached_collision_object_topic( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC) + , monitored_planning_scene_topic(planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC) + , publish_planning_scene_topic(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC) + { + } + void load(const rclcpp::Node::SharedPtr& node) { const std::string ns = "planning_scene_monitor_options"; node->get_parameter_or(ns + ".name", name, std::string("planning_scene_monitor")); node->get_parameter_or(ns + ".robot_description", robot_description, std::string("robot_description")); - node->get_parameter_or(ns + ".joint_state_topic", joint_state_topic, - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC); - node->get_parameter_or(ns + ".attached_collision_object_topic", attached_collision_object_topic, - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC); - node->get_parameter_or(ns + ".monitored_planning_scene_topic", monitored_planning_scene_topic, - planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC); - node->get_parameter_or(ns + ".publish_planning_scene_topic", publish_planning_scene_topic, - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC); node->get_parameter_or(ns + ".wait_for_initial_state_timeout", wait_for_initial_state_timeout, 0.0); } std::string name; std::string robot_description; - std::string joint_state_topic; - std::string attached_collision_object_topic; - std::string monitored_planning_scene_topic; - std::string publish_planning_scene_topic; + const std::string joint_state_topic; + const std::string attached_collision_object_topic; + const std::string monitored_planning_scene_topic; + const std::string publish_planning_scene_topic; double wait_for_initial_state_timeout; }; diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index 103a7e1d09..106a6fd663 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -866,7 +866,7 @@ void PlanningSceneMonitor::excludeRobotLinksFromOctree() bool warned = false; for (const moveit::core::LinkModel* link : links) { - std::vector shapes = link->getShapes(); // copy shared ptrs on purpuse + std::vector shapes = link->getShapes(); // copy shared ptrs on purpose for (std::size_t j = 0; j < shapes.size(); ++j) { // merge mesh vertices up to 0.1 mm apart diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index 02057f181b..be02e8c6d2 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -103,7 +103,7 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface MOVEIT_STRUCT_FORWARD(Plan); - /// The representation of a motion plan (as ROS messasges) + /// The representation of a motion plan (as ROS messages) struct Plan { /// The full starting state used for planning @@ -160,6 +160,9 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface * states from srdf */ const std::vector& getNamedTargets() const; + /** \brief Get the tf2_ros::Buffer. */ + const std::shared_ptr& getTF() const; + /** \brief Get the RobotModel object. */ moveit::core::RobotModelConstPtr getRobotModel() const; @@ -236,6 +239,9 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface If the value is greater than 1, it is set to 1.0. */ void setMaxVelocityScalingFactor(double max_velocity_scaling_factor); + /** \brief Get the max velocity scaling factor set by setMaxVelocityScalingFactor(). */ + double getMaxVelocityScalingFactor() const; + /** \brief Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0,1]. The maximum joint acceleration specified in the robot model is multiplied by the factor. If the value is 0, it is set to @@ -243,6 +249,9 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface If the value is greater than 1, it is set to 1.0. */ void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor); + /** \brief Get the max acceleration scaling factor set by setMaxAccelerationScalingFactor(). */ + double getMaxAccelerationScalingFactor() const; + /** \brief Get the number of seconds set by setPlanningTime() */ double getPlanningTime() const; @@ -811,6 +820,14 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface /** \brief How often is the system allowed to move the camera to update environment model when looking */ void setLookAroundAttempts(int32_t attempts); + /** \brief Build a RobotState message for use with plan() or computeCartesianPath() + * If the move_group has a custom set start state, this method will use that as the robot state. + * + * Otherwise, the robot state will be with `is_diff` set to true, causing it to be an offset from the current state + * of the robot at time of the state's use. + */ + void constructRobotState(moveit_msgs::msg::RobotState& state); + /** \brief Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and store it in \e request */ void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request); diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index e9762a9abd..84a46005ce 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -359,11 +359,21 @@ class MoveGroupInterface::MoveGroupInterfaceImpl setMaxScalingFactor(max_velocity_scaling_factor_, value, "velocity_scaling_factor", 0.1); } + double getMaxVelocityScalingFactor() const + { + return max_velocity_scaling_factor_; + } + void setMaxAccelerationScalingFactor(double value) { setMaxScalingFactor(max_acceleration_scaling_factor_, value, "acceleration_scaling_factor", 0.1); } + double getMaxAccelerationScalingFactor() const + { + return max_acceleration_scaling_factor_; + } + void setMaxScalingFactor(double& variable, const double target_value, const char* factor_name, double fallback_value) { if (target_value > 1.0) @@ -857,14 +867,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl auto req = std::make_shared(); moveit_msgs::srv::GetCartesianPath::Response::SharedPtr response; - if (considered_start_state_) - { - moveit::core::robotStateToRobotStateMsg(*considered_start_state_, req->start_state); - } - else - { - req->start_state.is_diff = true; - } + constructRobotState(req->start_state); req->group_name = opt_.group_name; req->header.frame_id = getPoseReferenceFrame(); @@ -1008,6 +1011,18 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return allowed_planning_time_; } + void constructRobotState(moveit_msgs::msg::RobotState& state) const + { + if (considered_start_state_) + { + moveit::core::robotStateToRobotStateMsg(*considered_start_state_, state); + } + else + { + state.is_diff = true; + } + } + void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request) const { request.group_name = opt_.group_name; @@ -1019,14 +1034,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl request.planner_id = planner_id_; request.workspace_parameters = workspace_parameters_; - if (considered_start_state_) - { - moveit::core::robotStateToRobotStateMsg(*considered_start_state_, request.start_state); - } - else - { - request.start_state.is_diff = true; - } + constructRobotState(request.start_state); if (active_target_ == JOINT) { @@ -1373,6 +1381,11 @@ const std::vector& MoveGroupInterface::getNamedTargets() const return impl_->getJointModelGroup()->getDefaultStateNames(); } +const std::shared_ptr& MoveGroupInterface::getTF() const +{ + return impl_->getTF(); +} + moveit::core::RobotModelConstPtr MoveGroupInterface::getRobotModel() const { return impl_->getRobotModel(); @@ -1440,11 +1453,21 @@ void MoveGroupInterface::setMaxVelocityScalingFactor(double max_velocity_scaling impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor); } +double MoveGroupInterface::getMaxVelocityScalingFactor() const +{ + return impl_->getMaxVelocityScalingFactor(); +} + void MoveGroupInterface::setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor) { impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor); } +double MoveGroupInterface::getMaxAccelerationScalingFactor() const +{ + return impl_->getMaxAccelerationScalingFactor(); +} + moveit::core::MoveItErrorCode MoveGroupInterface::asyncMove() { return impl_->move(false); @@ -2323,6 +2346,11 @@ bool MoveGroupInterface::detachObject(const std::string& name) return impl_->detachObject(name); } +void MoveGroupInterface::constructRobotState(moveit_msgs::msg::RobotState& state) +{ + impl_->constructRobotState(state); +} + void MoveGroupInterface::constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& goal_out) { impl_->constructMotionPlanRequest(goal_out); diff --git a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.hpp b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.hpp index 6dc958350e..bcaf78297d 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.hpp +++ b/moveit_setup_assistant/moveit_setup_controllers/include/moveit_setup_controllers/moveit_controllers.hpp @@ -60,8 +60,7 @@ class MoveItControllers : public Controllers std::string getInstructions() const override { - return "Configure controllers to be used in executing trajectories with MoveIt (hardware or simulation)." - "hardware"; + return "Configure controllers to be used in executing trajectories with MoveIt (hardware or simulation)."; } std::string getButtonText() const override diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/controllers_widget.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/controllers_widget.cpp index e902fc78e7..9580de2b3c 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/src/controllers_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_controllers/src/controllers_widget.cpp @@ -355,7 +355,7 @@ void ControllersWidget::deleteController() // Delete actual controller if (setup_step_->deleteController(controller_name)) { - RCLCPP_INFO_STREAM(setup_step_->getLogger(), "Controller " << controller_name << " deleted succefully"); + RCLCPP_INFO_STREAM(setup_step_->getLogger(), "Controller " << controller_name << " deleted successfully"); } else { diff --git a/moveit_setup_assistant/moveit_setup_controllers/templates/config/ros2_control.xacro b/moveit_setup_assistant/moveit_setup_controllers/templates/config/ros2_control.xacro index 3dcb96d963..c28dcc4710 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/templates/config/ros2_control.xacro +++ b/moveit_setup_assistant/moveit_setup_controllers/templates/config/ros2_control.xacro @@ -1,7 +1,7 @@ - + diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp index 2d907c92b5..9e9529fbbd 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/robot_poses.cpp @@ -51,7 +51,7 @@ void RobotPoses::onInit() // Set the planning scene // srdf_config_->getPlanningScene()->setName("MoveIt Planning Scene"); - // Collision Detection initializtion ------------------------------- + // Collision Detection initialization ------------------------------- // Setup the request request_.contacts = true;