Merge branch 'moveit:main' into main

Author: Demonmasterlqx

.github/mergify.yml

@@ -8,23 +8,23 @@ pull_request_rules:
         branches:
           - humble
 
-  - name: backport to iron at reviewers discretion
+  - name: backport to jazzy at reviewers discretion
     conditions:
       - base=main
-      - "label=backport-iron"
+      - "label=backport-jazzy"
     actions:
       backport:
         branches:
-          - iron
+          - jazzy
 
-  - name: backport to jazzy at reviewers discretion
+  - name: backport to kilted at reviewers discretion
     conditions:
       - base=main
-      - "label=backport-jazzy"
+      - "label=backport-kilted"
     actions:
       backport:
         branches:
-          - jazzy
+          - kilted
 
   - name: ask to resolve conflict
     conditions:

.github/workflows/ci.yaml

@@ -33,7 +33,6 @@ jobs:
       # https://stackoverflow.com/a/41673702
       CXXFLAGS: >-
         -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unknown-warning-option
-      CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file
       DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }}
       UPSTREAM_WORKSPACE: >
         moveit2.repos
@@ -143,7 +142,7 @@ jobs:
         run: moveit_kinematics/test/test_ikfast_plugins.sh
       - id: ici
         name: Run industrial_ci
-        uses: ros-industrial/industrial_ci@master
+        uses: ros-industrial/industrial_ci@ba2a3d0f830f8051b356711a8df2fedfc5d256cf
         env: ${{ matrix.env }}
 # NOTE: Testspace is temporarily disabled and needs to be installed for the MoveIt org
 # See: https://github.com/moveit/moveit2/issues/2852

.github/workflows/tutorial_docker.yaml

@@ -46,15 +46,17 @@ jobs:
       - name: "Remove .dockerignore"
         run: rm .dockerignore  # enforce full source context
       - name: Cache ccache
-        uses: actions/cache@v4
+        uses: actions/cache@v5
         with:
           path: .ccache
           key: docker-tutorial-ccache-${{ matrix.ROS_DISTRO }}-${{ hashFiles( '.docker/tutorial-source/Dockerfile' ) }}
       - name: inject ccache into docker
         uses: reproducible-containers/[email protected]
         with:
-          cache-source: .ccache
-          cache-target: /root/.ccache/
+           cache-map: |
+              {
+                ".ccache": "/root/.ccache/"
+              }
       - name: Build and Push
         uses: docker/build-push-action@v6
         with:

moveit_core/robot_model/src/planar_joint_model.cpp

@@ -355,6 +355,10 @@ void PlanarJointModel::computeVariablePositions(const Eigen::Isometry3d& transf,
 
   ASSERT_ISOMETRY(transf)  // unsanitized input, could contain a non-isometry
   Eigen::Quaterniond q(transf.linear());
+  if (q.w() < 0.0)
+  {
+    q.coeffs() *= -1.0;
+  }
   // taken from Bullet
   double s_squared = 1.0 - (q.w() * q.w());
   if (s_squared < 10.0 * std::numeric_limits<double>::epsilon())

moveit_core/robot_model/test/test.cpp

@@ -172,6 +172,38 @@ TEST(FloatingJointTest, interpolation_test)
   }
 }
 
+TEST(PlanarJointTest, ComputeVariablePositionsNormalizeYaw)
+{
+  // Create a simple planar joint model with some dummy parameters
+  moveit::core::PlanarJointModel pjm("joint", 0, 0);
+
+  // Test various angles for normalization
+  std::vector<double> test_angles = { 0.0, M_PI / 2, -M_PI / 2, M_PI, -M_PI, 3.0, -3.0, 4.1, -4.1 };
+  for (double angle : test_angles)
+  {
+    // Create a transform with the given yaw angle
+    Eigen::Isometry3d transform = Eigen::Isometry3d::Identity();
+    Eigen::AngleAxisd q = Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ());
+    transform.linear() = q.toRotationMatrix();
+
+    // Compute the variable positions from the transform
+    double joint_values[3];
+    pjm.computeVariablePositions(transform, joint_values);
+
+    // Check that the x and y values are zero (since we didn't set any translation)
+    EXPECT_NEAR(joint_values[0], 0.0, 1e-6);
+    EXPECT_NEAR(joint_values[1], 0.0, 1e-6);
+
+    // The yaw value should be normalized to the range [-pi, pi]
+    double normalized_angle = angle;
+    while (normalized_angle > M_PI)
+      normalized_angle -= 2.0 * M_PI;
+    while (normalized_angle < -M_PI)
+      normalized_angle += 2.0 * M_PI;
+    EXPECT_NEAR(joint_values[2], normalized_angle, 1e-6);
+  }
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);

moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.hpp

@@ -310,6 +310,16 @@ class RobotTrajectory
    *  @param The waypoint index before the supplied duration.
    *  @param The waypoint index after (or equal to) the supplied duration.
    *  @param The progress (0 to 1) between the two waypoints, based on time (not based on joint distances).
+   *
+   *  General Case:
+   *  - Usually after == before + 1, except in the edge cases listed below.
+   *  - blend 0.0 implies being exactly at the before waypoint.
+   *  - blend 1.0 implies being exactly at the after waypoint.
+   *  Edge Cases:
+   *  - Empty Trajectory: before == 0, after == 0, blend == 0.0
+   *  - Negative Duration: before == 0, after == 0, blend == 0.0
+   *  - Duration > Total Duration: before == size() - 1, after == size() - 1, blend == 1.0
+   *  - Single Waypoint Trajectory: before == 0, after == 0, blend == 1.0
    */
   void findWayPointIndicesForDurationAfterStart(double duration, int& before, int& after, double& blend) const;
 

moveit_core/robot_trajectory/src/robot_trajectory.cpp

@@ -484,8 +484,67 @@ RobotTrajectory& RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::Robo
     {
       for (std::size_t j = 0; j < trajectory.multi_dof_joint_trajectory.joint_names.size(); ++j)
       {
+        const auto& joint_name = trajectory.multi_dof_joint_trajectory.joint_names[j];
         Eigen::Isometry3d t = tf2::transformToEigen(trajectory.multi_dof_joint_trajectory.points[i].transforms[j]);
-        st->setJointPositions(trajectory.multi_dof_joint_trajectory.joint_names[j], t);
+        st->setJointPositions(joint_name, t);
+
+        if (!trajectory.multi_dof_joint_trajectory.points[i].velocities.empty())
+        {
+          // Note: angular x and y are not currently supported
+          const auto names = st->getVariableNames();
+
+          for (const auto& name : names)
+          {
+            if (name.find("/x") != std::string::npos)
+            {
+              st->setVariableVelocity(joint_name + "/x",
+                                      trajectory.multi_dof_joint_trajectory.points[i].velocities[j].linear.x);
+            }
+            else if (name.find("/y") != std::string::npos)
+            {
+              st->setVariableVelocity(joint_name + "/y",
+                                      trajectory.multi_dof_joint_trajectory.points[i].velocities[j].linear.y);
+            }
+            else if (name.find("/z") != std::string::npos)
+            {
+              st->setVariableVelocity(joint_name + "/z",
+                                      trajectory.multi_dof_joint_trajectory.points[i].velocities[j].linear.z);
+            }
+            else if (name.find("/theta") != std::string::npos)
+            {
+              st->setVariableVelocity(joint_name + "/theta",
+                                      trajectory.multi_dof_joint_trajectory.points[i].velocities[j].angular.z);
+            }
+          }
+        }
+        if (!trajectory.multi_dof_joint_trajectory.points[i].accelerations.empty())
+        {
+          // Note: angular x and y are not currently supported
+          const auto names = st->getVariableNames();
+          for (const auto& name : names)
+          {
+            if (name.find("/x") != std::string::npos)
+            {
+              st->setVariableAcceleration(joint_name + "/x",
+                                          trajectory.multi_dof_joint_trajectory.points[i].accelerations[j].linear.x);
+            }
+            else if (name.find("/y") != std::string::npos)
+            {
+              st->setVariableAcceleration(joint_name + "/y",
+                                          trajectory.multi_dof_joint_trajectory.points[i].accelerations[j].linear.y);
+            }
+            else if (name.find("/z") != std::string::npos)
+            {
+              st->setVariableAcceleration(joint_name + "/z",
+                                          trajectory.multi_dof_joint_trajectory.points[i].accelerations[j].linear.z);
+            }
+            else if (name.find("/theta") != std::string::npos)
+            {
+              st->setVariableAcceleration(joint_name + "/theta",
+                                          trajectory.multi_dof_joint_trajectory.points[i].accelerations[j].angular.z);
+            }
+          }
+        }
       }
       this_time_stamp = rclcpp::Time(trajectory.multi_dof_joint_trajectory.header.stamp) +
                         trajectory.multi_dof_joint_trajectory.points[i].time_from_start;
@@ -509,7 +568,7 @@ RobotTrajectory& RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::Robo
 void RobotTrajectory::findWayPointIndicesForDurationAfterStart(double duration, int& before, int& after,
                                                                double& blend) const
 {
-  if (duration < 0.0)
+  if (duration < 0.0 || waypoints_.empty())
   {
     before = 0;
     after = 0;
@@ -522,22 +581,22 @@ void RobotTrajectory::findWayPointIndicesForDurationAfterStart(double duration,
   double running_duration = 0.0;
   for (; index < num_points; ++index)
   {
-    running_duration += duration_from_previous_[index];
+    running_duration += duration_from_previous_.at(index);
     if (running_duration >= duration)
       break;
   }
   before = std::max<int>(index - 1, 0);
   after = std::min<int>(index, num_points - 1);
 
   // Compute duration blend
-  double before_time = running_duration - duration_from_previous_[index];
   if (after == before)
   {
     blend = 1.0;
   }
   else
   {
-    blend = (duration - before_time) / duration_from_previous_[index];
+    double before_time = running_duration - duration_from_previous_.at(index);
+    blend = (duration - before_time) / duration_from_previous_.at(index);
   }
 }