Merge branch 'humble' into mergify/bp/humble/pr-2601

.docker/ci/Dockerfile

@@ -6,6 +6,8 @@ FROM ros:${ROS_DISTRO}-ros-base
 LABEL maintainer Robert Haschke [email protected]
 
 ENV TERM xterm
+# Allow non-interactive installation of ros-humble-rmw-connextdds
+ENV RTI_NC_LICENSE_ACCEPTED yes
 
 # Setup (temporary) ROS workspace
 WORKDIR /root/ws_moveit

.docker/release/Dockerfile

@@ -5,6 +5,9 @@ ARG ROS_DISTRO=rolling
 FROM ros:${ROS_DISTRO}-ros-base
 LABEL maintainer Robert Haschke [email protected]
 
+# Allow non-interactive installation of ros-humble-rmw-connextdds
+ENV RTI_NC_LICENSE_ACCEPTED yes
+
 # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size
 RUN apt-get update -q && \
     apt-get upgrade -q -y && \

.github/workflows/format.yaml

@@ -16,7 +16,7 @@ jobs:
     runs-on: ubuntu-22.04
     steps:
     - uses: actions/checkout@v4
-    - uses: actions/setup-python@v4
+    - uses: actions/setup-python@v5
       with:
         python-version: '3.10'
     - name: Install clang-format-12

moveit2.repos

@@ -9,7 +9,3 @@ repositories:
     type: git
     url: https://github.com/ros-planning/moveit_resources.git
     version: humble
-  ros2_control:
-    type: git
-    url: https://github.com/ros-controls/ros2_control.git
-    version: 2.13.0

moveit_configs_utils/moveit_configs_utils/launches.py

@@ -197,7 +197,12 @@ def generate_move_group_launch(moveit_config):
         DeclareBooleanLaunchArg("publish_monitored_planning_scene", default_value=True)
     )
     # load non-default MoveGroup capabilities (space separated)
-    ld.add_action(DeclareLaunchArgument("capabilities", default_value=""))
+    ld.add_action(
+        DeclareLaunchArgument(
+            "capabilities",
+            default_value=moveit_config.move_group_capabilities["capabilities"],
+        )
+    )
     # inhibit these default MoveGroup capabilities (space separated)
     ld.add_action(DeclareLaunchArgument("disable_capabilities", default_value=""))
 
@@ -244,10 +249,12 @@ def generate_move_group_launch(moveit_config):
     return ld
 
 
-def generate_demo_launch(moveit_config):
+def generate_demo_launch(moveit_config, launch_package_path=None):
     """
     Launches a self contained demo
 
+    launch_package_path is optional to use different launch and config packages
+
     Includes
      * static_virtual_joint_tfs
      * robot_state_publisher
@@ -256,6 +263,9 @@ def generate_demo_launch(moveit_config):
      * warehouse_db (optional)
      * ros2_control_node + controller spawners
     """
+    if launch_package_path == None:
+        launch_package_path = moveit_config.package_path
+
     ld = LaunchDescription()
     ld.add_action(
         DeclareBooleanLaunchArg(
@@ -272,11 +282,11 @@ def generate_demo_launch(moveit_config):
         )
     )
     ld.add_action(DeclareBooleanLaunchArg("use_rviz", default_value=True))
-
     # If there are virtual joints, broadcast static tf by including virtual_joints launch
     virtual_joints_launch = (
-        moveit_config.package_path / "launch/static_virtual_joint_tfs.launch.py"
+        launch_package_path / "launch/static_virtual_joint_tfs.launch.py"
     )
+
     if virtual_joints_launch.exists():
         ld.add_action(
             IncludeLaunchDescription(
@@ -288,15 +298,15 @@ def generate_demo_launch(moveit_config):
     ld.add_action(
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
-                str(moveit_config.package_path / "launch/rsp.launch.py")
+                str(launch_package_path / "launch/rsp.launch.py")
             ),
         )
     )
 
     ld.add_action(
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
-                str(moveit_config.package_path / "launch/move_group.launch.py")
+                str(launch_package_path / "launch/move_group.launch.py")
             ),
         )
     )
@@ -305,7 +315,7 @@ def generate_demo_launch(moveit_config):
     ld.add_action(
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
-                str(moveit_config.package_path / "launch/moveit_rviz.launch.py")
+                str(launch_package_path / "launch/moveit_rviz.launch.py")
             ),
             condition=IfCondition(LaunchConfiguration("use_rviz")),
         )
@@ -315,7 +325,7 @@ def generate_demo_launch(moveit_config):
     ld.add_action(
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
-                str(moveit_config.package_path / "launch/warehouse_db.launch.py")
+                str(launch_package_path / "launch/warehouse_db.launch.py")
             ),
             condition=IfCondition(LaunchConfiguration("db")),
         )
@@ -336,7 +346,7 @@ def generate_demo_launch(moveit_config):
     ld.add_action(
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
-                str(moveit_config.package_path / "launch/spawn_controllers.launch.py")
+                str(launch_package_path / "launch/spawn_controllers.launch.py")
             ),
         )
     )

moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py

@@ -162,6 +162,7 @@ def __init__(
 
         self.__urdf_package = None
         self.__urdf_file_path = None
+        self.__urdf_xacro_args = None
         self.__srdf_file_path = None
 
         modified_urdf_path = Path("config") / (self.__robot_name + ".urdf.xacro")
@@ -172,15 +173,20 @@ def __init__(
         if setup_assistant_file.exists():
             setup_assistant_yaml = load_yaml(setup_assistant_file)
             config = setup_assistant_yaml.get("moveit_setup_assistant_config", {})
-            urdf_config = config.get("urdf", config.get("URDF"))
-            if urdf_config and self.__urdf_package is None:
-                self.__urdf_package = Path(
-                    get_package_share_directory(urdf_config["package"])
-                )
-                self.__urdf_file_path = Path(urdf_config["relative_path"])
 
-            srdf_config = config.get("srdf", config.get("SRDF"))
-            if srdf_config:
+            if urdf_config := config.get("urdf", config.get("URDF")):
+                if self.__urdf_package is None:
+                    self.__urdf_package = Path(
+                        get_package_share_directory(urdf_config["package"])
+                    )
+                    self.__urdf_file_path = Path(urdf_config["relative_path"])
+
+                if xacro_args := urdf_config.get("xacro_args"):
+                    self.__urdf_xacro_args = dict(
+                        arg.split(":=") for arg in xacro_args.split(" ") if arg
+                    )
+
+            if srdf_config := config.get("srdf", config.get("SRDF")):
                 self.__srdf_file_path = Path(srdf_config["relative_path"])
 
         if not self.__urdf_package or not self.__urdf_file_path:
@@ -224,7 +230,8 @@ def robot_description(
             try:
                 self.__moveit_configs.robot_description = {
                     self.__robot_description: load_xacro(
-                        robot_description_file_path, mappings=mappings
+                        robot_description_file_path,
+                        mappings=mappings or self.__urdf_xacro_args,
                     )
                 }
             except ParameterBuilderFileNotFoundError as e:

moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h

@@ -406,7 +406,7 @@ class JointModelGroup
   void interpolate(const double* from, const double* to, double t, double* state) const;
 
   /** \brief Get the number of variables that describe this joint group. This includes variables necessary for mimic
-      joints, so will always be >= the number of items returned by getActiveVariableNames() */
+      joints, so will always be >= getActiveVariableCount() */
   unsigned int getVariableCount() const
   {
     return variable_count_;

moveit_core/robot_model/src/floating_joint_model.cpp

@@ -36,6 +36,7 @@
 /* Author: Ioan Sucan */
 
 #include <cmath>
+#include <Eigen/Geometry>
 #include <geometric_shapes/check_isometry.h>
 #include <limits>
 #include <moveit/robot_model/floating_joint_model.h>
@@ -121,12 +122,10 @@ double FloatingJointModel::distanceTranslation(const double* values1, const doub
 
 double FloatingJointModel::distanceRotation(const double* values1, const double* values2) const
 {
-  double dq =
-      fabs(values1[3] * values2[3] + values1[4] * values2[4] + values1[5] * values2[5] + values1[6] * values2[6]);
-  if (dq + std::numeric_limits<double>::epsilon() >= 1.0)
-    return 0.0;
-  else
-    return acos(dq);
+  // The values are in "xyzw" order but Eigen expects "wxyz".
+  const auto q1 = Eigen::Quaterniond(values1[6], values1[3], values1[4], values1[5]).normalized();
+  const auto q2 = Eigen::Quaterniond(values2[6], values2[3], values2[4], values2[5]).normalized();
+  return q2.angularDistance(q1);
 }
 
 void FloatingJointModel::interpolate(const double* from, const double* to, const double t, double* state) const

moveit_core/robot_trajectory/test/test_robot_trajectory.cpp

@@ -54,7 +54,7 @@ class RobotTrajectoryTestFixture : public testing::Test
   {
     robot_model_ = moveit::core::loadTestingRobotModel(robot_model_name_);
     robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
-    robot_state_->setToDefaultValues(arm_jmg_name_, arm_state_name_);
+    robot_state_->setToDefaultValues();
     robot_state_->setVariableVelocity(/*index*/ 0, /*value*/ 1.0);
     robot_state_->setVariableAcceleration(/*index*/ 0, /*value*/ -0.1);
     robot_state_->update();
@@ -66,7 +66,7 @@ class RobotTrajectoryTestFixture : public testing::Test
 
   void initTestTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory)
   {
-    // Init a traj
+    // Init a trajectory
     ASSERT_TRUE(robot_model_->hasJointModelGroup(arm_jmg_name_))
         << "Robot model does not have group: " << arm_jmg_name_;
 
@@ -78,7 +78,10 @@ class RobotTrajectoryTestFixture : public testing::Test
     double duration_from_previous = 0.1;
     std::size_t waypoint_count = 5;
     for (std::size_t ix = 0; ix < waypoint_count; ++ix)
+    {
       trajectory->addSuffixWayPoint(*robot_state_, duration_from_previous);
+    }
+
     // Quick check that getDuration is working correctly
     EXPECT_EQ(trajectory->getDuration(), duration_from_previous * waypoint_count)
         << "Generated trajectory duration incorrect";
@@ -302,6 +305,17 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryLength)
 {
   robot_trajectory::RobotTrajectoryPtr trajectory;
   initTestTrajectory(trajectory);
+  EXPECT_FLOAT_EQ(robot_trajectory::path_length(*trajectory), 0.0);
+
+  // modify joint values so the smoothness is nonzero
+  std::vector<double> positions;
+  for (size_t i = 0; i < trajectory->size(); ++i)
+  {
+    auto waypoint = trajectory->getWayPointPtr(i);
+    waypoint->copyJointGroupPositions(arm_jmg_name_, positions);
+    positions[0] += 0.01 * i;
+    waypoint->setJointGroupPositions(arm_jmg_name_, positions);
+  }
   EXPECT_GT(robot_trajectory::path_length(*trajectory), 0.0);
 }
 
@@ -310,6 +324,16 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectorySmoothness)
   robot_trajectory::RobotTrajectoryPtr trajectory;
   initTestTrajectory(trajectory);
 
+  // modify joint values so the smoothness is nonzero
+  std::vector<double> positions;
+  for (size_t i = 0; i < trajectory->size(); ++i)
+  {
+    auto waypoint = trajectory->getWayPointPtr(i);
+    waypoint->copyJointGroupPositions(arm_jmg_name_, positions);
+    positions[0] += 0.01 * i;
+    waypoint->setJointGroupPositions(arm_jmg_name_, positions);
+  }
+
   const auto smoothness = robot_trajectory::smoothness(*trajectory);
   ASSERT_TRUE(smoothness.has_value());
   EXPECT_GT(smoothness.value(), 0.0);
@@ -324,13 +348,28 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDensity)
   robot_trajectory::RobotTrajectoryPtr trajectory;
   initTestTrajectory(trajectory);
 
-  const auto density = robot_trajectory::waypoint_density(*trajectory);
+  // If trajectory has all equal state, and length zero, density should be null.
+  auto density = robot_trajectory::waypoint_density(*trajectory);
+  ASSERT_FALSE(density.has_value());
+
+  // modify joint values so the density is nonzero
+  std::vector<double> positions;
+  for (size_t i = 0; i < trajectory->size(); ++i)
+  {
+    auto waypoint = trajectory->getWayPointPtr(i);
+    waypoint->copyJointGroupPositions(arm_jmg_name_, positions);
+    positions[0] += 0.01 * i;
+    waypoint->setJointGroupPositions(arm_jmg_name_, positions);
+  }
+
+  density = robot_trajectory::waypoint_density(*trajectory);
   ASSERT_TRUE(density.has_value());
   EXPECT_GT(density.value(), 0.0);
 
   // Check for empty trajectory
   trajectory->clear();
-  EXPECT_FALSE(robot_trajectory::waypoint_density(*trajectory).has_value());
+  density = robot_trajectory::waypoint_density(*trajectory);
+  EXPECT_FALSE(density.has_value());
 }
 
 int main(int argc, char** argv)

moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp

@@ -324,6 +324,12 @@ Trajectory::Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, co
   , time_step_(time_step)
   , cached_time_(std::numeric_limits<double>::max())
 {
+  if (time_step_ == 0)
+  {
+    valid_ = false;
+    RCLCPP_ERROR(LOGGER, "The trajectory is invalid because the time step is 0.");
+    return;
+  }
   trajectory_.push_back(TrajectoryStep(0.0, 0.0));
   double after_acceleration = getMinMaxPathAcceleration(0.0, 0.0, true);
   while (valid_ && !integrateForward(trajectory_, after_acceleration) && valid_)
@@ -565,6 +571,16 @@ bool Trajectory::integrateForward(std::list<TrajectoryStep>& trajectory, double
     trajectory.push_back(TrajectoryStep(path_pos, path_vel));
     acceleration = getMinMaxPathAcceleration(path_pos, path_vel, true);
 
+    if (path_vel == 0 && acceleration == 0)
+    {
+      // The position will never change if velocity and acceleration are zero.
+      // The loop will spin indefinitely as no exit condition is met.
+      valid_ = false;
+      RCLCPP_ERROR(LOGGER, "Error while integrating forward: zero acceleration and velocity. Are any relevant "
+                           "acceleration components limited to zero?");
+      return true;
+    }
+
     if (path_vel > getAccelerationMaxPathVelocity(path_pos) || path_vel > getVelocityMaxPathVelocity(path_pos))
     {
       // Find more accurate intersection with max-velocity curve using bisection

moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp

@@ -496,6 +496,36 @@ TEST(time_optimal_trajectory_generation, testPluginAPI)
   ASSERT_EQ(first_trajectory_msg_end, third_trajectory_msg_end);
 }
 
+TEST(time_optimal_trajectory_generation, testRelevantZeroMaxAccelerationsInvalidateTrajectory)
+{
+  const Eigen::Vector2d max_velocity(1, 1);
+  const Path path(std::list<Eigen::VectorXd>{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 1) });
+
+  EXPECT_FALSE(Trajectory(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(0, 1)).isValid());
+  EXPECT_FALSE(Trajectory(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(1, 0)).isValid());
+  EXPECT_FALSE(Trajectory(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(0, 0)).isValid());
+}
+
+TEST(time_optimal_trajectory_generation, testIrrelevantZeroMaxAccelerationsDontInvalidateTrajectory)
+{
+  const Eigen::Vector2d max_velocity(1, 1);
+
+  EXPECT_TRUE(Trajectory(Path(std::list<Eigen::VectorXd>{ Eigen::Vector2d(0, 0), Eigen::Vector2d(0, 1) }), max_velocity,
+                         /*max_acceleration=*/Eigen::Vector2d(0, 1))
+                  .isValid());
+  EXPECT_TRUE(Trajectory(Path(std::list<Eigen::VectorXd>{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 0) }), max_velocity,
+                         /*max_acceleration=*/Eigen::Vector2d(1, 0))
+                  .isValid());
+}
+
+TEST(time_optimal_trajectory_generation, testTimeStepZeroMakesTrajectoryInvalid)
+{
+  EXPECT_FALSE(Trajectory(Path(std::list<Eigen::VectorXd>{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 1) }),
+                          /*max_velocity=*/Eigen::Vector2d(1, 1), /*max_acceleration=*/Eigen::Vector2d(1, 1),
+                          /*time_step=*/0)
+                   .isValid());
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);