From be007e9bd63dfdb7f89eafad6100aae0981ed92e Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Tue, 25 Jun 2024 10:45:00 +0000 Subject: [PATCH 1/5] =?UTF-8?q?=F0=9F=9B=A0=EF=B8=8F=20Bump=20SonarSource/?= =?UTF-8?q?sonarcloud-github-c-cpp=20from=202=20to=203?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Bumps [SonarSource/sonarcloud-github-c-cpp](https://github.com/sonarsource/sonarcloud-github-c-cpp) from 2 to 3. - [Release notes](https://github.com/sonarsource/sonarcloud-github-c-cpp/releases) - [Commits](https://github.com/sonarsource/sonarcloud-github-c-cpp/compare/v2...v3) --- updated-dependencies: - dependency-name: SonarSource/sonarcloud-github-c-cpp dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] --- .github/workflows/sonar.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/sonar.yaml b/.github/workflows/sonar.yaml index 083d6c07ca..8febc84e0c 100644 --- a/.github/workflows/sonar.yaml +++ b/.github/workflows/sonar.yaml @@ -110,7 +110,7 @@ jobs: run: cat ${{ github.workspace }}/.work/target_ws/coverage.info - name: Install sonar-scanner if: steps.ici.outputs.target_test_results == '0' - uses: SonarSource/sonarcloud-github-c-cpp@v2 + uses: SonarSource/sonarcloud-github-c-cpp@v3 - name: Run sonar-scanner if: steps.ici.outputs.target_test_results == '0' env: From b147d7ee44b497b4df508dab12c0a85dd9e869ea Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 19 Jul 2024 11:55:05 +0200 Subject: [PATCH 2/5] Fix CHOMP segfault (#3621) * Prevent segfault when getParentLinkModel() is NULL * Simpliy code * Add unit test operating the panda gripper --------- Co-authored-by: Captain Yoshi --- .../chomp_interface/test/chomp_moveit.test | 7 -- .../test/chomp_moveit_panda.test | 9 +++ .../test/chomp_moveit_rrbot.test | 7 ++ .../test/chomp_moveit_test_panda.cpp | 80 +++++++++++++++++++ ...t_test.cpp => chomp_moveit_test_rrbot.cpp} | 2 +- .../src/chomp_optimizer.cpp | 27 ++----- 6 files changed, 105 insertions(+), 27 deletions(-) delete mode 100644 moveit_planners/chomp/chomp_interface/test/chomp_moveit.test create mode 100644 moveit_planners/chomp/chomp_interface/test/chomp_moveit_panda.test create mode 100644 moveit_planners/chomp/chomp_interface/test/chomp_moveit_rrbot.test create mode 100644 moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_panda.cpp rename moveit_planners/chomp/chomp_interface/test/{chomp_moveit_test.cpp => chomp_moveit_test_rrbot.cpp} (98%) diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit.test b/moveit_planners/chomp/chomp_interface/test/chomp_moveit.test deleted file mode 100644 index e965ff64b3..0000000000 --- a/moveit_planners/chomp/chomp_interface/test/chomp_moveit.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_panda.test b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_panda.test new file mode 100644 index 0000000000..952f150350 --- /dev/null +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_panda.test @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_rrbot.test b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_rrbot.test new file mode 100644 index 0000000000..47c4dfe61b --- /dev/null +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_rrbot.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_panda.cpp b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_panda.cpp new file mode 100644 index 0000000000..0d84b43521 --- /dev/null +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_panda.cpp @@ -0,0 +1,80 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Sherbrooke University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/// \author Captain Yoshi + +#include +#include + +#include +#include + +class CHOMPMoveitTest : public ::testing::Test +{ +public: + moveit::planning_interface::MoveGroupInterface move_group_; + moveit::planning_interface::MoveGroupInterface::Plan my_plan_; + +public: + CHOMPMoveitTest() : move_group_("hand") + { + } +}; + +// TEST CASES + +// https://github.com/moveit/moveit/issues/2542 +TEST_F(CHOMPMoveitTest, jointSpaceGoodGoal) +{ + move_group_.setStartState(*(move_group_.getCurrentState())); + move_group_.setJointValueTarget(std::vector({ 0.0, 0.0 })); + move_group_.setPlanningTime(5.0); + + moveit::core::MoveItErrorCode error_code = move_group_.plan(my_plan_); + EXPECT_GT(my_plan_.trajectory_.joint_trajectory.points.size(), 0u); + EXPECT_EQ(error_code.val, moveit::core::MoveItErrorCode::SUCCESS); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "chomp_moveit_test_panda"); + + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_rrbot.cpp similarity index 98% rename from moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp rename to moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_rrbot.cpp index 8e79efa70b..7e69f51c41 100644 --- a/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp +++ b/moveit_planners/chomp/chomp_interface/test/chomp_moveit_test_rrbot.cpp @@ -107,7 +107,7 @@ TEST_F(CHOMPMoveitTest, collisionAtEndOfPath) int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "chomp_moveit_test"); + ros::init(argc, argv, "chomp_moveit_test_rrbot"); ros::AsyncSpinner spinner(1); spinner.start(); diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp index d1fc2c2091..23ead931c0 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp @@ -205,26 +205,15 @@ void ChompOptimizer::initialize() { if (fixed_link_resolution_map.find(link->getParentJointModel()->getName()) == fixed_link_resolution_map.end()) { - const moveit::core::JointModel* parent_model = nullptr; - bool found_root = false; - - while (!found_root) + const moveit::core::JointModel* parent_model = link->getParentJointModel(); + while (true) // traverse up the tree until we find a joint we know about in joint_names_ { - if (parent_model == nullptr) - { - parent_model = link->getParentJointModel(); - } - else - { - parent_model = parent_model->getParentLinkModel()->getParentJointModel(); - for (const std::string& joint_name : joint_names_) - { - if (parent_model->getName() == joint_name) - { - found_root = true; - } - } - } + if (parent_model->getParentLinkModel() == nullptr) + break; + + parent_model = parent_model->getParentLinkModel()->getParentJointModel(); + if (std::find(joint_names_.begin(), joint_names_.end(), parent_model->getName()) != joint_names_.end()) + break; } fixed_link_resolution_map[link->getParentJointModel()->getName()] = parent_model->getName(); } From 2b8a067cff74e1eb4f09a71e873b34055c1abfa0 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 19 Jul 2024 12:47:46 +0200 Subject: [PATCH 3/5] CHOMP: Fix handling of mimic joints (#3625) As the CHOMP planner only considers active joints, it needs to use setJointGroupActivePositions() instead of setJointGroupPositions(). --- .../chomp/chomp_motion_planner/src/chomp_optimizer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp index 23ead931c0..f31f48510d 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp @@ -952,7 +952,7 @@ void ChompOptimizer::setRobotStateFromPoint(ChompTrajectory& group_trajectory, i for (size_t j = 0; j < group_trajectory.getNumJoints(); ++j) joint_states.emplace_back(point(0, j)); - state_.setJointGroupPositions(planning_group_, joint_states); + state_.setJointGroupActivePositions(planning_group_, joint_states); state_.update(); } From a0a5a633fe7ceba1f7bce2e7e363bf8fb7310525 Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Fri, 19 Jul 2024 06:49:45 -0400 Subject: [PATCH 4/5] Optimize MOVE_SHAPE operations for FCL (#3601) Don't recreate the FCL object when moving shapes, but just update its transforms. --- .../test_collision_common_panda.h | 13 ++++++++ .../src/collision_env_fcl.cpp | 32 +++++++++++++++++++ 2 files changed, 45 insertions(+) diff --git a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h index 92163406aa..061a304085 100644 --- a/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h +++ b/moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h @@ -155,11 +155,24 @@ TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_1) ASSERT_TRUE(res.collision); res.clear(); + pos1.translation().z() = 0.25; + this->cenv_->getWorld()->moveObject("box", pos1); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + res.clear(); + + pos1.translation().z() = 0.05; this->cenv_->getWorld()->moveObject("box", pos1); this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); ASSERT_TRUE(res.collision); res.clear(); + pos1.translation().z() = 0.25; + this->cenv_->getWorld()->moveObject("box", pos1); + this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_); + ASSERT_FALSE(res.collision); + res.clear(); + this->cenv_->getWorld()->moveObject("box", pos1); ASSERT_FALSE(res.collision); } diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp index 54a007d325..86a48b8320 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -450,6 +450,38 @@ void CollisionEnvFCL::notifyObjectChange(const ObjectConstPtr& obj, World::Actio } cleanCollisionGeometryCache(); } + else if (action == World::MOVE_SHAPE) + { + auto it = fcl_objs_.find(obj->id_); + if (it == fcl_objs_.end()) + { + RCLCPP_ERROR(getLogger(), "Cannot move shapes of unknown FCL object: '%s'", obj->id_.c_str()); + return; + } + + if (obj->global_shape_poses_.size() != it->second.collision_objects_.size()) + { + RCLCPP_ERROR(getLogger(), + "Cannot move shapes, shape size mismatch between FCL object and world object: '%s'. Respectively " + "%zu and %zu.", + obj->id_.c_str(), it->second.collision_objects_.size(), it->second.collision_objects_.size()); + return; + } + + for (std::size_t i = 0; i < it->second.collision_objects_.size(); ++i) + { + it->second.collision_objects_[i]->setTransform(transform2fcl(obj->global_shape_poses_[i])); + + // compute AABB, order matters + it->second.collision_geometry_[i]->collision_geometry_->computeLocalAABB(); + it->second.collision_objects_[i]->computeAABB(); + } + + // update AABB in the FCL broadphase manager tree + // see https://github.com/moveit/moveit/pull/3601 for benchmarks + it->second.unregisterFrom(manager_.get()); + it->second.registerTo(manager_.get()); + } else { updateFCLObject(obj->id_); From 955f308f166b07b9c1a4fbf34bab922a9968ee0b Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Fri, 2 Aug 2024 10:26:48 -0400 Subject: [PATCH 5/5] Deduplicate joint trajectory points in Pilz Move Group Sequence capability (#2943) * Deduplicate joint trajectory points before sending them to controllers * Fix max loop value * Move deduplication code to Pilz * Clean up * Add todo --- .../robot_trajectory/robot_trajectory.h | 11 ++++++++++ .../cartesian_trajectory_point.h | 1 - .../src/command_list_manager.cpp | 21 ++++++++++++++++++- 3 files changed, 31 insertions(+), 2 deletions(-) diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index 1f77946234..ed2887e2a2 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -244,6 +244,17 @@ class RobotTrajectory void swap(robot_trajectory::RobotTrajectory& other) noexcept; + /** + * \brief Remove a point from the trajectory + * \param index - the index to remove + */ + RobotTrajectory& removeWayPoint(std::size_t index) + { + waypoints_.erase(waypoints_.begin() + index); + duration_from_previous_.erase(duration_from_previous_.begin() + index); + return *this; + } + RobotTrajectory& clear() { waypoints_.clear(); diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h index 4835dc8876..e810ae4726 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h @@ -38,7 +38,6 @@ #include #include -#include namespace pilz_industrial_motion_planner { diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp index be11aa84da..de829de34a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp @@ -112,7 +112,26 @@ RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConst // therefore: "i-1". (i > 0 ? radii.at(i - 1) : 0.)); } - return plan_comp_builder_.build(); + + const auto res_vec = plan_comp_builder_.build(); + + // De-duplicate trajectory points with the same time value. + // This is necessary since some controllers do not allow times that are not monotonically increasing. + // TODO: Ideally, we would not need this code if the trajectory segments were created without + // duplicate time points in the first place. Leaving this note to revisit this with a more principled fix. + for (const auto& traj : res_vec) + { + for (size_t i = 0; i < traj->size() - 1; ++i) + { + if (traj->getWayPointDurationFromStart(i) == traj->getWayPointDurationFromStart(i + 1)) + { + RCLCPP_WARN(getLogger(), "Removed duplicate point at time=%f", traj->getWayPointDurationFromStart(i)); + traj->removeWayPoint(i + 1); + } + } + } + + return res_vec; } bool CommandListManager::checkRadiiForOverlap(const robot_trajectory::RobotTrajectory& traj_A, const double radii_A,