Merge branch 'main' into ch3/trajectory-cache-refactor

.github/workflows/ci.yaml

@@ -34,7 +34,7 @@ jobs:
       # TODO(andyz): When this clang-tidy issue is fixed, remove -Wno-unknown-warning-option
       # https://stackoverflow.com/a/41673702
       CXXFLAGS: >-
-        -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unknown-warning-option
+        -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unknown-warning-option -Wno-cpp
       CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file
       DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }}
       UPSTREAM_WORKSPACE: >
@@ -105,36 +105,36 @@ jobs:
         with:
           file: moveit2.repos
       - name: Cache upstream workspace
-        uses: actions/cache@v4
+        uses: rhaschke/cache@main
         with:
           path: ${{ env.BASEDIR }}/upstream_ws
           key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }}
           restore-keys: ${{ env.CACHE_PREFIX }}
-          save-always: true
         env:
+          GHA_CACHE_SAVE: always
           CACHE_PREFIX: ${{ steps.rosdistro_release_date.outputs.date }}-upstream_ws-${{ steps.repos_edit_timestamp.outputs.timestamp }}-${{ matrix.env.IMAGE }}-${{ hashFiles('moveit2*.repos', '.github/workflows/ci.yaml') }}
       # The target directory cache doesn't include the source directory because
       # that comes from the checkout.  See "prepare target_ws for cache" task below
       - name: Cache target workspace
         if: "!matrix.env.CCOV"
-        uses: actions/cache@v4
+        uses: rhaschke/cache@main
         with:
           path: ${{ env.BASEDIR }}/target_ws
           key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }}
           restore-keys: ${{ env.CACHE_PREFIX }}
-          save-always: true
         env:
+          GHA_CACHE_SAVE: always
           CACHE_PREFIX: target_ws${{ matrix.env.CCOV && '-ccov' || '' }}-${{ matrix.env.IMAGE }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml', '.github/workflows/ci.yaml') }}
       - name: Cache ccache
-        uses: actions/cache@v4
+        uses: rhaschke/cache@main
         with:
           path: ${{ env.CCACHE_DIR }}
           key: ${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
           restore-keys: |
             ${{ env.CACHE_PREFIX }}-${{ github.sha }}
             ${{ env.CACHE_PREFIX }}
-          save-always: true
         env:
+          GHA_CACHE_SAVE: always
           CACHE_PREFIX: ccache-${{ matrix.env.IMAGE }}${{ matrix.env.CCOV && '-ccov' || '' }}
       - name: Configure ccache
         run: |

.github/workflows/format.yaml

@@ -19,7 +19,7 @@ jobs:
     - uses: actions/checkout@v4
     - uses: actions/setup-python@v5
       with:
-        python-version: '3.10'
+        python-version: 3.x
     - name: Install clang-format-14
       run: sudo apt-get install clang-format-14
     - uses: pre-commit/[email protected]

.github/workflows/sonar.yaml

@@ -72,23 +72,24 @@ jobs:
         with:
           file: moveit2.repos
       - name: Cache upstream workspace
-        uses: actions/cache@v4
+        uses: rhaschke/cache@main
         with:
           path: ${{ env.BASEDIR }}/upstream_ws
           key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }}
           restore-keys: ${{ env.CACHE_PREFIX }}
-          save-always: true
         env:
+          GHA_CACHE_SAVE: always
           CACHE_PREFIX: ${{ steps.rosdistro_release_date.outputs.date }}-upstream_ws-${{ steps.repos_edit_timestamp.outputs.timestamp }}-rolling-ci-${{ hashFiles('moveit2*.repos', '.github/workflows/ci.yaml') }}
       - name: Cache ccache
-        uses: actions/cache@v4
+        uses: rhaschke/cache@main
         with:
           path: ${{ env.CCACHE_DIR }}
           key: ccache-rolling-ci-ccov-${{ github.sha }}-${{ github.run_id }}
           restore-keys: |
             ccache-rolling-ci-ccov-${{ github.sha }}
             ccache-rolling-ci-ccov
-          save-always: true
+        env:
+          GHA_CACHE_SAVE: always
       - name: Configure ccache
         run: |
           mkdir -p ${{ env.CCACHE_DIR }}

moveit2.repos

@@ -12,3 +12,7 @@ repositories:
     type: git
     url: https://github.com/moveit/moveit_resources.git
     version: ros2
+  srdfdom:
+    type: git
+    url: https://github.com/moveit/srdfdom.git
+    version: ros2

moveit_configs_utils/moveit_configs_utils/launches.py

@@ -204,7 +204,12 @@ def generate_move_group_launch(moveit_config):
         )
     )
     # inhibit these default MoveGroup capabilities (space separated)
-    ld.add_action(DeclareLaunchArgument("disable_capabilities", default_value=""))
+    ld.add_action(
+        DeclareLaunchArgument(
+            "disable_capabilities",
+            default_value=moveit_config.move_group_capabilities["disable_capabilities"],
+        )
+    )
 
     # do not copy dynamics information from /joint_states to internal robot monitoring
     # default to false, because almost nothing in move_group relies on this information

moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py

@@ -108,7 +108,9 @@ class MoveItConfigs:
     # A dictionary that has the sensor 3d configuration parameters.
     sensors_3d: Dict = field(default_factory=dict)
     # A dictionary containing move_group's non-default capabilities.
-    move_group_capabilities: Dict = field(default_factory=dict)
+    move_group_capabilities: Dict = field(
+        default_factory=lambda: {"capabilities": "", "disable_capabilities": ""}
+    )
     # A dictionary containing the overridden position/velocity/acceleration limits.
     joint_limits: Dict = field(default_factory=dict)
     # A dictionary containing MoveItCpp related parameters.

moveit_core/constraint_samplers/test/pr2_arm_ik.h

@@ -37,7 +37,11 @@
 #pragma once
 
 #include <urdf_model/model.h>
+#if __has_include(<urdf/model.hpp>)
+#include <urdf/model.hpp>
+#else
 #include <urdf/model.h>
+#endif
 #include <Eigen/Geometry>
 #include <Eigen/LU>  // provides LU decomposition
 #include <kdl/chainiksolver.hpp>

moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h

@@ -50,7 +50,11 @@
 
 #include <kdl/chainfksolverpos_recursive.hpp>
 
+#if __has_include(<urdf/model.hpp>)
+#include <urdf/model.hpp>
+#else
 #include <urdf/model.h>
+#endif
 
 #include <moveit/kinematics_base/kinematics_base.h>
 

moveit_core/robot_state/src/cartesian_interpolator.cpp

@@ -285,23 +285,22 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath(
     double wp_percentage_solved =
         computeCartesianPath(start_state, group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step,
                              precision, validCallback, options, cost_function, link_offset);
+
+    std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
+    if (i > 0 && !waypoint_traj.empty())
+      std::advance(start, 1);
+    traj.insert(traj.end(), start, waypoint_traj.end());
+
     if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits<double>::epsilon())
     {
       percentage_solved = static_cast<double>(i + 1) / static_cast<double>(waypoints.size());
-      std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
-      if (i > 0 && !waypoint_traj.empty())
-        std::advance(start, 1);
-      traj.insert(traj.end(), start, waypoint_traj.end());
     }
     else
     {
       percentage_solved += wp_percentage_solved / static_cast<double>(waypoints.size());
-      std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
-      if (i > 0 && !waypoint_traj.empty())
-        std::advance(start, 1);
-      traj.insert(traj.end(), start, waypoint_traj.end());
       break;
     }
+    start_state = traj.back().get();
   }
 
   return percentage_solved;
@@ -490,21 +489,19 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath(
         computeCartesianPath(start_state, group, waypoint_path, link, waypoints[i], global_reference_frame, max_step,
                              NO_JOINT_SPACE_JUMP_TEST, validCallback, options, cost_function, link_offset);
 #pragma GCC diagnostic pop
+
+    std::vector<RobotStatePtr>::iterator start = waypoint_path.begin();
+    if (i > 0 && !waypoint_path.empty())
+      std::advance(start, 1);
+    path.insert(path.end(), start, waypoint_path.end());
+
     if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits<double>::epsilon())
     {
       percentage_solved = static_cast<double>((i + 1)) / static_cast<double>(waypoints.size());
-      std::vector<RobotStatePtr>::iterator start = waypoint_path.begin();
-      if (i > 0 && !waypoint_path.empty())
-        std::advance(start, 1);
-      path.insert(path.end(), start, waypoint_path.end());
     }
     else
     {
       percentage_solved += wp_percentage_solved / static_cast<double>(waypoints.size());
-      std::vector<RobotStatePtr>::iterator start = waypoint_path.begin();
-      if (i > 0 && !waypoint_path.empty())
-        std::advance(start, 1);
-      path.insert(path.end(), start, waypoint_path.end());
       break;
     }
   }

moveit_core/utils/include/moveit/utils/robot_model_test_utils.h

@@ -38,7 +38,11 @@
 #pragma once
 
 #include <srdfdom/srdf_writer.h>
+#if __has_include(<urdf/model.hpp>)
+#include <urdf/model.hpp>
+#else
 #include <urdf/model.h>
+#endif
 #include <moveit/robot_model/robot_model.h>
 #include <geometry_msgs/msg/pose.hpp>
 

moveit_planners/moveit_planners/package.xml

@@ -19,10 +19,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <!--
   <exec_depend>chomp_motion_planner</exec_depend>
-  <exec_depend>moveit_planners_chomp</exec_depend>
-  -->
   <exec_depend>moveit_planners_ompl</exec_depend>
   <exec_depend>moveit_planners_stomp</exec_depend>
   <exec_depend>pilz_industrial_motion_planner</exec_depend>

moveit_py/moveit/planning.pyi

@@ -16,7 +16,7 @@ class MoveItPy:
     def get_planning_component(self, *args, **kwargs) -> Any: ...
     def get_planning_scene_monitor(self, *args, **kwargs) -> Any: ...
     def get_robot_model(self, *args, **kwargs) -> Any: ...
-    def get_trajactory_execution_manager(self, *args, **kwargs) -> Any: ...
+    def get_trajectory_execution_manager(self, *args, **kwargs) -> Any: ...
     def shutdown(self, *args, **kwargs) -> Any: ...
 
 class MultiPipelinePlanRequestParameters:

moveit_ros/moveit_servo/config/panda_simulated_config.yaml

@@ -28,7 +28,7 @@ publish_joint_accelerations: false
 
 ## Plugins for smoothing outgoing commands
 use_smoothing: true
-smoothing_filter_plugin_name: "online_signal_smoothing::RuckigFilterPlugin"
+smoothing_filter_plugin_name: "online_signal_smoothing::AccelerationLimitedPlugin"
 
 # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
 # which other nodes can use as a source for information about the planning environment.

moveit_ros/moveit_servo/config/servo_parameters.yaml

@@ -86,24 +86,23 @@ servo:
     linear: {
       type: double,
       default_value: 0.4,
-      description: "Max linear velocity. Unit is [m/s]. Only used for Cartesian commands."
+      description: "Max linear velocity in m/s if using speed units or tracking a pose, else a scaling factor on the unitless commanded velocity. Used for Cartesian and pose tracking commands."
     }
     rotational: {
       type: double,
       default_value: 0.8,
-      description: "Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands."
+      description: "Max angular velocity in rad/s if using speed units or tracking a pose, else a scaling factor on the unitless commanded velocity. Used for Cartesian and pose tracking commands."
     }
     joint: {
       type: double,
       default_value: 0.5,
-      description: "Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic."
+      description: "Joint angular/linear velocity scale. Only used for unitless joint commands."
     }
 
-
   incoming_command_timeout: {
     type: double,
     default_value: 0.1,
-    description: "Commands will be discarded if it is older than the timeout.\
+    description: "Commands will be discarded if they are older than the timeout, in seconds.\
                    Important because ROS may drop some messages."
   }
 
@@ -123,7 +122,7 @@ servo:
     linear_tolerance: {
         type: double,
         default_value: 0.001,
-        description: "The allowable linear error when tracking a pose.",
+        description: "The allowable linear error, in meters, when tracking a pose.",
         validation: {
           gt<>: 0.0
         }
@@ -132,7 +131,7 @@ servo:
     angular_tolerance: {
         type: double,
         default_value: 0.01,
-        description: "The allowable angular error when tracking a pose.",
+        description: "The allowable angular error, in radians, when tracking a pose.",
         validation: {
           gt<>: 0.0
         }

moveit_ros/moveit_servo/config/test_config_panda.yaml

@@ -12,10 +12,10 @@ incoming_command_timeout: 0.5 # seconds
 command_in_type: "unitless" # "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:  1.0  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
-  rotational:  1.0 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
+  linear: 0.2  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
+  rotational: 0.2  # 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: 1.0
+  joint: 0.5
 
 # What type of topic does your robot driver expect?
 # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
@@ -56,7 +56,7 @@ status_topic: ~/status # Publish status to this topic
 command_out_topic: /panda_arm_controller/joint_trajectory # Publish outgoing commands here
 
 ## Collision checking for the entire robot body
-check_collisions: true # Check collisions?
+check_collisions: true  # Check collisions?
 collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
 self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
 scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]

moveit_ros/moveit_servo/package.xml

@@ -45,6 +45,7 @@
   <exec_depend>joy</exec_depend>
   <exec_depend>launch_param_builder</exec_depend>
   <exec_depend>moveit_configs_utils</exec_depend>
+  <exec_depend>moveit_ros_visualization</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>tf2_ros</exec_depend>