Merge pull request #9 from sbgisen/feature/revert_rendering

Revert "MotionPlanningDisplay: always update query states"
sbgisen · Jul 31, 2023 · 3a9e63a · 3a9e63a
2 parents b0f4556 + 5095dbd
commit 3a9e63a
Show file tree

Hide file tree

Showing 72 changed files with 196 additions and 66 deletions.
diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile
@@ -34,7 +34,7 @@ RUN \
     git clone --depth 1 --branch master https://github.com/ros-planning/moveit_resources src/moveit_resources && \
     #
     # Download all dependencies of MoveIt
-    rosdep update && \
+    rosdep update $(test "${ROS_DISTRO}" = "melodic" && echo "--include-eol-distros") && \
     DEBIAN_FRONTEND=noninteractive \
     rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false && \
     # Remove the source code from this container

diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml
@@ -99,14 +99,16 @@ jobs:
           no-cache: ${{ steps.apt.outputs.no_cache || github.event_name == 'workflow_dispatch' }}
           cache-from: type=registry,ref=${{ env.IMAGE }}
           cache-to: type=inline
-          tags: ${{ env.IMAGE }}
+          tags: |
+            ${{ env.IMAGE }}
+            ${{ matrix.IMAGE == 'master' && 'moveit/moveit:master-ci-testing' || '' }}
 
   ci-testing:
     needs: ci
     strategy:
       fail-fast: false
       matrix:
-        IMAGE: [noetic, master]
+        IMAGE: [noetic]
     runs-on: ubuntu-latest
     permissions:
       packages: write

diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.13 (2023-07-28)
+-------------------
+
 1.1.12 (2023-05-13)
 -------------------
 

diff --git a/moveit/package.xml b/moveit/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>moveit</name>
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <description>Meta package that contains all essential package of MoveIt. Until Summer 2016 MoveIt had been developed over multiple repositories, where developers' usability and maintenance effort was non-trivial. See <a href = "http://discourse.ros.org/t/migration-to-one-github-repo-for-moveit/266/34">the detailed discussion for the merge of several repositories</a>.</description>
   <maintainer email="dave@picknik.ai">Dave Coleman</maintainer>
   <maintainer email="mferguson@fetchrobotics.com">Michael Ferguson</maintainer>

diff --git a/moveit_commander/CHANGELOG.rst b/moveit_commander/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit_commander
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.13 (2023-07-28)
+-------------------
+
 1.1.12 (2023-05-13)
 -------------------
 * Rename argument of execute() to trajectory (`#3392 <https://github.com/ros-planning/moveit/issues/3392>`_)

diff --git a/moveit_commander/package.xml b/moveit_commander/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>moveit_commander</name>
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <description>Python interfaces to MoveIt</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>

diff --git a/moveit_core/CHANGELOG.rst b/moveit_core/CHANGELOG.rst
@@ -2,6 +2,18 @@
 Changelog for package moveit_core
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.13 (2023-07-28)
+-------------------
+* Avoid global transforms in ``getRigidlyConnectedParentLinkModel()`` (`#3470 <https://github.com/ros-planning/moveit/issues/3470>`_)
+
+  * RobotState::setFromIK: ensure up-to-date state before calling IK solver
+  * Remove unimplemented RobotState::getSubframeTransformInLinkFrame()
+
+* Add missing include (`#3451 <https://github.com/ros-planning/moveit/issues/3451>`_)
+* Fix Jacobian calculation for planar joint (`#3439 <https://github.com/ros-planning/moveit/issues/3439>`_)
+* Silent "empty quaternion" warning from poseMsgToEigen() (`#3435 <https://github.com/ros-planning/moveit/issues/3435>`_)
+* Contributors: Cong Liu, Ivo Vatavuk, Robert Haschke
+
 1.1.12 (2023-05-13)
 -------------------
 * Generalize RobotState::setFromIK() (`<https://github.com/ros-planning/moveit/issues/3388>`_)

diff --git a/moveit_core/package.xml b/moveit_core/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>moveit_core</name>
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <description>Core libraries used by MoveIt</description>
   <author email="isucan@google.com">Ioan Sucan</author>
   <author email="robot.moveit@gmail.com">Sachin Chitta</author>

diff --git a/moveit_core/robot_model/include/moveit/robot_model/robot_model.h b/moveit_core/robot_model/include/moveit/robot_model/robot_model.h
@@ -271,7 +271,14 @@ class RobotModel
    * will actually warp wrist (and all its descendants).
    */
   static const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const LinkModel* link,
+                                                                           Eigen::Isometry3d& transform,
                                                                            const JointModelGroup* jmg = nullptr);
+  static const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const LinkModel* link,
+                                                                           const JointModelGroup* jmg = nullptr)
+  {
+    Eigen::Isometry3d unused;
+    return getRigidlyConnectedParentLinkModel(link, unused, jmg);
+  }
 
   /** \brief Get the array of links  */
   const std::vector<const LinkModel*>& getLinkModels() const

diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp
@@ -1265,10 +1265,13 @@ LinkModel* RobotModel::getLinkModel(const std::string& name, bool* has_link)
   return nullptr;
 }
 
-const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* link, const JointModelGroup* jmg)
+const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel* link, Eigen::Isometry3d& transform,
+                                                                const JointModelGroup* jmg)
 {
   if (!link)
     return link;
+
+  transform.setIdentity();
   const moveit::core::LinkModel* parent_link = link->getParentLinkModel();
   const moveit::core::JointModel* joint = link->getParentJointModel();
   decltype(jmg->getJointModels().cbegin()) begin{}, end{};
@@ -1289,6 +1292,7 @@ const LinkModel* RobotModel::getRigidlyConnectedParentLinkModel(const LinkModel*
 
   while (parent_link && is_fixed_or_not_in_jmg(joint))
   {
+    transform = link->getJointOriginTransform() * transform;
     link = parent_link;
     joint = link->getParentJointModel();
     parent_link = joint->getParentLinkModel();

diff --git a/moveit_core/robot_state/include/moveit/robot_state/attached_body.h b/moveit_core/robot_state/include/moveit/robot_state/attached_body.h
@@ -175,13 +175,6 @@ class AttachedBody
    * The returned transform is guaranteed to be a valid isometry. */
   const Eigen::Isometry3d& getSubframeTransform(const std::string& frame_name, bool* found = nullptr) const;
 
-  /** \brief Get the fixed transform to a named subframe on this body (relative to the robot link)
-   *
-   * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's
-   * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false).
-   * The returned transform is guaranteed to be a valid isometry. */
-  const Eigen::Isometry3d& getSubframeTransformInLinkFrame(const std::string& frame_name, bool* found = nullptr) const;
-
   /** \brief Get the fixed transform to a named subframe on this body, relative to the world frame.
    * The frame_name needs to have the object's name prepended (e.g. "screwdriver/tip" returns true if the object's
    * name is "screwdriver"). Returns an identity transform if frame_name is unknown (and set found to false).

diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h
@@ -1379,7 +1379,7 @@ class RobotState
    * This behaves the same as RobotModel::getRigidlyConnectedParentLinkModel,
    * but can additionally resolve parents for attached objects / subframes.
    *
-   * If transform is specified, return the relative transform from the returned parent link to frame.
+   * If transform is specified, return the (fixed) relative transform from the returned parent link to frame.
    */
   const moveit::core::LinkModel*
   getRigidlyConnectedParentLinkModel(const std::string& frame, Eigen::Isometry3d* transform = nullptr,

diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp
@@ -160,10 +160,7 @@ double CartesianInterpolator::computeCartesianPath(RobotState* start_state, cons
     // Explicitly use a single IK attempt only: We want a smooth trajectory.
     // Random seeding (of additional attempts) would probably create IK jumps.
     if (start_state->setFromIK(group, pose * offset, link->getName(), consistency_limits, 0.0, validCallback, options))
-    {
-      start_state->update();
       traj.push_back(std::make_shared<moveit::core::RobotState>(*start_state));
-    }
     else
       break;
 

diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp
@@ -809,36 +809,38 @@ const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::strin
     if (!hasAttachedBody(object))
       return nullptr;
     auto body{ getAttachedBody(object) };
-    if (!body->hasSubframeTransform(frame))
-      return nullptr;
+    bool found = false;
     if (transform)
-      *transform = body->getGlobalSubframeTransform(frame);
+      *transform = body->getSubframeTransform(frame, &found);
+    else
+      body->getSubframeTransform(frame, &found);
+    if (!found)
+      return nullptr;
+    if (transform)  // prepend the body transform
+      *transform = body->getPose() * *transform;
     link = body->getAttachedLink();
   }
   else if (hasAttachedBody(frame))
   {
     auto body{ getAttachedBody(frame) };
     if (transform)
-      *transform = body->getGlobalPose();
+      *transform = body->getPose();
     link = body->getAttachedLink();
   }
   else if (getRobotModel()->hasLinkModel(frame))
   {
     link = getLinkModel(frame);
     if (transform)
-    {
-      BOOST_VERIFY(checkLinkTransforms());
-      *transform = global_link_transforms_[link->getLinkIndex()];
-    }
+      transform->setIdentity();
+    if (!link)
+      return nullptr;
   }
   // link is valid and transform describes pose of frame w.r.t. global frame
-  auto* parent = getRobotModel()->getRigidlyConnectedParentLinkModel(link, jmg);
+  Eigen::Isometry3d link_transform;
+  auto* parent = getRobotModel()->getRigidlyConnectedParentLinkModel(link, link_transform, jmg);
   if (parent && transform)
-  {
-    BOOST_VERIFY(checkLinkTransforms());
-    // compute transform from parent link to frame
-    *transform = global_link_transforms_[parent->getLinkIndex()].inverse() * *transform;
-  }
+    // prepend link_transform to get transform from parent link to frame
+    *transform = link_transform * *transform;
   return parent;
 }
 
@@ -1654,6 +1656,9 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is
   else if (consistency_limit_sets.size() == 1)
     consistency_limits = consistency_limit_sets[0];
 
+  // ensure RobotState is up-to-date before employing it in the IK solver
+  update(false);
+
   const std::vector<std::string>& solver_tip_frames = solver->getTipFrames();
 
   // Track which possible tips frames we have filled in so far

diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp
@@ -686,7 +686,6 @@ TEST_F(OneRobot, rigidlyConnectedParent)
   EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_b), link_a);
 
   moveit::core::RobotState state(robot_model_);
-  state.update();
 
   Eigen::Isometry3d a_to_b;
   EXPECT_EQ(state.getRigidlyConnectedParentLinkModel("link_b", &a_to_b), link_a);

diff --git a/...tory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/...tory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h
@@ -42,6 +42,7 @@
 #include <list>
 #include <moveit/robot_trajectory/robot_trajectory.h>
 #include <moveit/trajectory_processing/time_parameterization.h>
+#include <unordered_map>
 
 namespace trajectory_processing
 {

diff --git a/moveit_kinematics/CHANGELOG.rst b/moveit_kinematics/CHANGELOG.rst
@@ -2,6 +2,15 @@
 Changelog for package moveit_kinematics
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.13 (2023-07-28)
+-------------------
+* Improvements to create_ikfast_moveit_plugin.py (`#3449 <https://github.com/ros-planning/moveit/issues/3449>`_)
+  * Make SRDF sanity checks optional
+  * Provide hint how to change kinematics.yaml if automatic adaption fails
+  * Modernize string formatting to Python 3.x
+  * Create new planning group entry in kinematics.yaml if needed
+* Contributors: Robert Haschke
+
 1.1.12 (2023-05-13)
 -------------------
 * Fix deprecation warnings in Debian bookworm (`#3397 <https://github.com/ros-planning/moveit/issues/3397>`_)

diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py b/moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py
@@ -427,7 +427,7 @@ def update_moveit_package(args):
             )
 
         groups = [g.get("name") for g in srdf.findall("group")]
-        if args.planing_group_name not in groups:
+        if args.planning_group_name not in groups:
             raise RuntimeWarning(
                 f"Planning group '{args.planning_group_name}' not defined in the SRDF."
                 " Available groups: \n" + ", ".join(groups)

diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>moveit_kinematics</name>
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <description>Package for all inverse kinematics solvers in MoveIt</description>
 
   <author email="dave@picknik.ai">Dave Coleman</author>

diff --git a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package chomp_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.13 (2023-07-28)
+-------------------
+
 1.1.12 (2023-05-13)
 -------------------
 * Drop lib/ prefix from plugin paths (`#3305 <https://github.com/ros-planning/moveit/issues/3305>`_)

diff --git a/moveit_planners/chomp/chomp_interface/package.xml b/moveit_planners/chomp/chomp_interface/package.xml
@@ -2,7 +2,7 @@
   <name>moveit_planners_chomp</name>
   <description>The interface for using CHOMP within MoveIt</description>
 
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <author email="gjones@willowgarage.com">Gil Jones</author>
 
   <maintainer email="chitt@live.in">Chittaranjan Srinivas Swaminathan</maintainer>

diff --git a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package chomp_motion_planner
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.13 (2023-07-28)
+-------------------
+
 1.1.12 (2023-05-13)
 -------------------
 

diff --git a/moveit_planners/chomp/chomp_motion_planner/package.xml b/moveit_planners/chomp/chomp_motion_planner/package.xml
@@ -2,7 +2,7 @@
   <name>chomp_motion_planner</name>
   <description>chomp_motion_planner</description>
 
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <author email="gjones@willowgarage.com">Gil Jones</author>
   <author email="mail@mrinal.net">Mrinal Kalakrishnan</author>
 

diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst b/moveit_planners/chomp/chomp_optimizer_adapter/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit_chomp_optimizer_adapter
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.13 (2023-07-28)
+-------------------
+
 1.1.12 (2023-05-13)
 -------------------
 

diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml
@@ -3,7 +3,7 @@
   <name>moveit_chomp_optimizer_adapter</name>
   <description>MoveIt planning request adapter utilizing chomp for solution optimization</description>
 
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <author email="raghavendersahdev@gmail.com">Raghavender Sahdev</author>
   <maintainer email="raghavendersahdev@gmail.com">Raghavender Sahdev</maintainer>
   <maintainer email="moveit_releasers@googlegroups.com">MoveIt Release Team</maintainer>

diff --git a/moveit_planners/moveit_planners/CHANGELOG.rst b/moveit_planners/moveit_planners/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit_planners
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.13 (2023-07-28)
+-------------------
+
 1.1.12 (2023-05-13)
 -------------------
 

diff --git a/moveit_planners/moveit_planners/package.xml b/moveit_planners/moveit_planners/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_planners</name>
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <description>Metapacakge that installs all available planners for MoveIt</description>
   <author email="isucan@google.com">Ioan Sucan</author>
   <author email="sachinc@willowgarage.com">Sachin Chitta</author>

diff --git a/moveit_planners/ompl/CHANGELOG.rst b/moveit_planners/ompl/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit_planners_ompl
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.13 (2023-07-28)
+-------------------
+
 1.1.12 (2023-05-13)
 -------------------
 * Add AITstar, BITstar and ABITstar planners from OMPL >= 1.5 (`#3347 <https://github.com/ros-planning/moveit/issues/3347>`_)

diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml
@@ -1,6 +1,6 @@
 <package format="2">
   <name>moveit_planners_ompl</name>
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <description>MoveIt interface to OMPL</description>
 
   <author email="isucan@google.com">Ioan Sucan</author>

diff --git a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package pilz_industrial_motion_planner
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.13 (2023-07-28)
+-------------------
+
 1.1.12 (2023-05-13)
 -------------------
 * Fix (some) doxygen warnings (`#3315 <https://github.com/ros-planning/moveit/issues/3315>`_)

diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>pilz_industrial_motion_planner</name>
-  <version>1.1.12</version>
+  <version>1.1.13</version>
   <description>MoveIt plugin to generate industrial trajectories PTP, LIN, CIRC and sequences thereof.</description>
 
   <maintainer email="a.gutenkunst@pilz.de">Alexander Gutenkunst</maintainer>