Merge branch 'moveit:main' into main

.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:

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();

moveit_planners/chomp/chomp_interface/test/chomp_moveit_panda.test

@@ -0,0 +1,9 @@
+<launch>
+
+  <include file="$(find moveit_resources_panda_moveit_config)/launch/test_environment.launch">
+    <arg name="pipeline" value="chomp" />
+  </include>
+
+  <test test-name="chomp_test_panda" pkg="moveit_planners_chomp" type="chomp_moveit_test_panda" time-limit="20.0"/>
+
+</launch>

moveit_planners/chomp/chomp_interface/test/chomp_moveit_rrbot.test

@@ -0,0 +1,7 @@
+<launch>
+
+  <include file="$(find moveit_planners_chomp)/test/rrbot_move_group.launch"/>
+
+  <test test-name="chomp_test_rrbot" pkg="moveit_planners_chomp" type="chomp_moveit_test_rrbot" time-limit="80.0"/>
+
+</launch>

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 <ros/ros.h>
+#include <gtest/gtest.h>
+
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+
+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<double>({ 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;
+}

moveit_planners/chomp/chomp_interface/test/chomp_moveit_test.cpp → 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();

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();
 }
 

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h

@@ -38,7 +38,6 @@
 
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/twist.hpp>
-#include <geometry_msgs/msg/twist.hpp>
 
 namespace pilz_industrial_motion_planner
 {

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,

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

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<std::string>& getNamedTargets() const;
 
+  /** \brief Get the tf2_ros::Buffer. */
+  const std::shared_ptr<tf2_ros::Buffer>& getTF() const;
+
   /** \brief Get the RobotModel object. */
   moveit::core::RobotModelConstPtr getRobotModel() const;
 

moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp

@@ -1382,6 +1382,11 @@ const std::vector<std::string>& MoveGroupInterface::getNamedTargets() const
   return impl_->getJointModelGroup()->getDefaultStateNames();
 }
 
+const std::shared_ptr<tf2_ros::Buffer>& MoveGroupInterface::getTF() const
+{
+  return impl_->getTF();
+}
+
 moveit::core::RobotModelConstPtr MoveGroupInterface::getRobotModel() const
 {
   return impl_->getRobotModel();

moveit_setup_assistant/moveit_setup_controllers/templates/config/ros2_control.xacro

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
     <xacro:macro name="[ROBOT_NAME]_ros2_control" params="name initial_positions_file">
-        <xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)['initial_positions']}"/>
+        <xacro:property name="initial_positions" value="${xacro.load_yaml(initial_positions_file)['initial_positions']}"/>
 
         <ros2_control name="${name}" type="system">
             <hardware>