diff --git a/.github/workflows/sonar.yaml b/.github/workflows/sonar.yaml index 083d6c07cac..8febc84e0cd 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/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index 1f779462340..ed2887e2a2f 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 e965ff64b38..00000000000 --- 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 00000000000..952f150350b --- /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 00000000000..47c4dfe61b9 --- /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 00000000000..0d84b435215 --- /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 8e79efa70ba..7e69f51c41f 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 d1fc2c2091b..f31f48510d6 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/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 4835dc8876a..e810ae47267 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 be11aa84da1..de829de34ae 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_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index b5d57bfcb97..50379995131 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/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 87f03685e05..4f82643017c 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -160,6 +160,9 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface * states from srdf */ const std::vector& getNamedTargets() const; + /** \brief Get the tf2_ros::Buffer. */ + const std::shared_ptr& getTF() const; + /** \brief Get the RobotModel object. */ moveit::core::RobotModelConstPtr getRobotModel() const; diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 028819a611c..3fbf9e3c634 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -1382,6 +1382,11 @@ const std::vector& MoveGroupInterface::getNamedTargets() const return impl_->getJointModelGroup()->getDefaultStateNames(); } +const std::shared_ptr& MoveGroupInterface::getTF() const +{ + return impl_->getTF(); +} + moveit::core::RobotModelConstPtr MoveGroupInterface::getRobotModel() const { return impl_->getRobotModel(); 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 3dcb96d963a..c28dcc4710f 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 @@ - +