Merge moveit/moveit@0d7462f

.github/workflows/ci.yaml

@@ -136,7 +136,7 @@ jobs:
           path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml
       - name: Generate codecov report
         uses: rhaschke/lcov-action@main
-        if: matrix.env.CCOV && steps.ici.outputs.target_test_results == '0'
+        if: always() && matrix.env.CCOV && steps.ici.outputs.target_test_results == '0'
         with:
           docker: $DOCKER_IMAGE
           workdir: ${{ env.BASEDIR }}/target_ws
@@ -153,7 +153,7 @@ jobs:
           name: clang-tidy
           path: ${{ env.BASEDIR }}/target_ws/src/$(basename $(pwd))
       - name: Prepare target_ws for cache
-        if: always() && ! matrix.env.CCOV
+        if: always() && !matrix.env.CCOV
         run: |
           du -sh ${{ env.BASEDIR }}/target_ws
           sudo find ${{ env.BASEDIR }}/target_ws -wholename '*/test_results/*' -delete

moveit_commander/src/moveit_commander/conversions.py

@@ -39,7 +39,7 @@
     # Use Python 3.x behaviour as fallback and choose the non-unicode version
     from io import BytesIO as StringIO
 
-from moveit_commander import MoveItCommanderException
+from .exception import MoveItCommanderException
 from geometry_msgs.msg import Pose, PoseStamped, Transform
 import rospy
 import tf

moveit_commander/src/moveit_commander/robot.py

@@ -32,7 +32,8 @@
 #
 # Author: Ioan Sucan
 
-from moveit_commander import MoveGroupCommander, MoveItCommanderException
+from moveit_commander import MoveGroupCommander
+from .exception import MoveItCommanderException
 from moveit_ros_planning_interface import _moveit_robot_interface
 from moveit_msgs.msg import RobotState
 from visualization_msgs.msg import MarkerArray

moveit_core/robot_state/include/moveit/robot_state/attached_body.h

@@ -60,10 +60,12 @@ class AttachedBody
   /** \brief Construct an attached body for a specified \e link.
    *
    * The name of this body is \e id and it consists of \e shapes that attach to the link by the transforms
-   * \e shape_poses. The set of links that are allowed to be touched by this object is specified by \e touch_links. */
-  AttachedBody(const LinkModel* link, const std::string& id, const Eigen::Isometry3d& pose,
+   * \e shape_poses. The set of links that are allowed to be touched by this object is specified by \e touch_links.
+   * detach_posture may describe a detach motion for the gripper when placing the object.
+   * The shape and subframe poses are relative to the \e pose, and \e pose is relative to the parent link. */
+  AttachedBody(const LinkModel* parent, const std::string& id, const Eigen::Isometry3d& pose,
                const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Isometry3d& shape_poses,
-               const std::set<std::string>& touch_links, const trajectory_msgs::msg::JointTrajectory& attach_posture,
+               const std::set<std::string>& touch_links, const trajectory_msgs::msg::JointTrajectory& detach_posture,
                const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap());
 
   ~AttachedBody();

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

@@ -1312,6 +1312,13 @@ class RobotState
   /** \brief Update the state after setting a particular link to the input global transform pose.*/
   void updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward = false);
 
+  /** \brief Get the latest link upwards the kinematic tree which is only connected via fixed joints.
+   *
+   * This behaves the same as RobotModel::getRigidlyConnectedParentLinkModel,
+   * but can additionally resolve parents for attached objects / subframes.
+   */
+  const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const std::string& frame) const;
+
   /** \brief Get the link transform w.r.t. the root link (model frame) of the RobotModel.
    *   This is typically the root link of the URDF unless a virtual joint is present.
    *   Checks the cache and if there are any dirty (non-updated) transforms, first updates them as needed.

moveit_core/robot_state/src/attached_body.cpp

@@ -43,12 +43,12 @@ namespace moveit
 {
 namespace core
 {
-AttachedBody::AttachedBody(const LinkModel* parent_link_model, const std::string& id, const Eigen::Isometry3d& pose,
+AttachedBody::AttachedBody(const LinkModel* parent, const std::string& id, const Eigen::Isometry3d& pose,
                            const std::vector<shapes::ShapeConstPtr>& shapes,
                            const EigenSTL::vector_Isometry3d& shape_poses, const std::set<std::string>& touch_links,
                            const trajectory_msgs::msg::JointTrajectory& detach_posture,
                            const FixedTransformsMap& subframe_poses)
-  : parent_link_model_(parent_link_model)
+  : parent_link_model_(parent)
   , id_(id)
   , pose_(pose)
   , shapes_(shapes)

moveit_core/robot_state/src/robot_state.cpp

@@ -45,7 +45,6 @@
 #else
 #include <tf2_eigen/tf2_eigen.h>
 #endif
-
 #include <moveit/macros/console_colors.h>
 #include <boost/bind.hpp>
 #include <moveit/robot_model/aabb.h>
@@ -799,6 +798,31 @@ void RobotState::updateStateWithLinkAt(const LinkModel* link, const Eigen::Isome
     it->second->computeTransform(global_link_transforms_[it->second->getAttachedLink()->getLinkIndex()]);
 }
 
+const LinkModel* RobotState::getRigidlyConnectedParentLinkModel(const std::string& frame) const
+{
+  const moveit::core::LinkModel* link{ nullptr };
+
+  size_t idx = 0;
+  if ((idx = frame.find('/')) != std::string::npos)
+  {  // resolve sub frame
+    std::string object{ frame.substr(0, idx) };
+    if (!hasAttachedBody(object))
+      return nullptr;
+    auto body{ getAttachedBody(object) };
+    if (!body->hasSubframeTransform(frame))
+      return nullptr;
+    link = body->getAttachedLink();
+  }
+  else if (hasAttachedBody(frame))
+  {
+    link = getAttachedBody(frame)->getAttachedLink();
+  }
+  else if (getRobotModel()->hasLinkModel(frame))
+    link = getLinkModel(frame);
+
+  return getRobotModel()->getRigidlyConnectedParentLinkModel(link);
+}
+
 bool RobotState::satisfiesBounds(double margin) const
 {
   const std::vector<const JointModel*>& jm = robot_model_->getActiveJointModels();

moveit_core/robot_state/test/robot_state_test.cpp

@@ -685,6 +685,39 @@ TEST_F(OneRobot, testInterpolation)
   EXPECT_TRUE(nan_exception) << "NaN interpolation parameter did not create expected exception.";
 }
 
+TEST_F(OneRobot, rigidlyConnectedParent)
+{
+  // link_e is its own rigidly-connected parent
+  const moveit::core::LinkModel* link_e{ robot_model_->getLinkModel("link_e") };
+  EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_e), link_e);
+
+  // link_b is rigidly connected to its parent link_a
+  const moveit::core::LinkModel* link_a{ robot_model_->getLinkModel("link_a") };
+  const moveit::core::LinkModel* link_b{ robot_model_->getLinkModel("link_b") };
+  EXPECT_EQ(robot_model_->getRigidlyConnectedParentLinkModel(link_b), link_a);
+
+  moveit::core::RobotState state(robot_model_);
+
+  EXPECT_EQ(state.getRigidlyConnectedParentLinkModel("link_b"), link_a);
+
+  // attach "object" with "subframe" to link_b
+  state.attachBody(new moveit::core::AttachedBody(
+      link_b, "object", Eigen::Isometry3d::Identity(), std::vector<shapes::ShapeConstPtr>{},
+      EigenSTL::vector_Isometry3d{}, std::set<std::string>{}, trajectory_msgs::msg::JointTrajectory{},
+      moveit::core::FixedTransformsMap{ { "subframe", Eigen::Isometry3d::Identity() } }));
+
+  // RobotState's version should resolve these too
+  EXPECT_EQ(link_a, state.getRigidlyConnectedParentLinkModel("object"));
+  EXPECT_EQ(link_a, state.getRigidlyConnectedParentLinkModel("object/subframe"));
+
+  // test failure cases
+  EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("no_object"));
+  EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("object/no_subframe"));
+  EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel(""));
+  EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("object/"));
+  EXPECT_EQ(nullptr, state.getRigidlyConnectedParentLinkModel("/"));
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);

moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp

@@ -935,17 +935,27 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
     max_velocity[j] = 1.0;
     if (bounds.velocity_bounded_)
     {
+      if (bounds.max_velocity_ < std::numeric_limits<double>::epsilon())
+      {
+        RCLCPP_ERROR(LOGGER, "Invalid max_velocity %f specified for '%s', must be greater than 0.0",
+                     bounds.max_velocity_, vars[j].c_str());
+        return false;
+      }
       max_velocity[j] =
           std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_)) * velocity_scaling_factor;
-      max_velocity[j] = std::max(0.01, max_velocity[j]);
     }
 
     max_acceleration[j] = 1.0;
     if (bounds.acceleration_bounded_)
     {
+      if (bounds.max_acceleration_ < std::numeric_limits<double>::epsilon())
+      {
+        RCLCPP_ERROR(LOGGER, "Invalid max_acceleration %f specified for '%s', must be greater than 0.0",
+                     bounds.max_acceleration_, vars[j].c_str());
+        return false;
+      }
       max_acceleration[j] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) *
                             acceleration_scaling_factor;
-      max_acceleration[j] = std::max(0.01, max_acceleration[j]);
     }
   }
 

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h

@@ -63,7 +63,7 @@ class PlanningContextBase : public planning_interface::PlanningContext
     , terminated_(false)
     , model_(model)
     , limits_(limits)
-    , generator_(model, limits_)
+    , generator_(model, limits_, group)
   {
   }
 

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h

@@ -81,7 +81,8 @@ class TrajectoryGeneratorCIRC : public TrajectoryGenerator
    *
    */
   TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr& robot_model,
-                          const pilz_industrial_motion_planner::LimitsContainer& planner_limits);
+                          const pilz_industrial_motion_planner::LimitsContainer& planner_limits,
+                          const std::string& group_name);
 
 private:
   void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override;

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h

@@ -66,7 +66,8 @@ class TrajectoryGeneratorLIN : public TrajectoryGenerator
    * @param planner_limits: limits in joint and Cartesian spaces
    */
   TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr& robot_model,
-                         const pilz_industrial_motion_planner::LimitsContainer& planner_limits);
+                         const pilz_industrial_motion_planner::LimitsContainer& planner_limits,
+                         const std::string& group_name);
 
 private:
   void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h

@@ -61,7 +61,8 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator
    * @param model: a map of joint limits information
    */
   TrajectoryGeneratorPTP(const moveit::core::RobotModelConstPtr& robot_model,
-                         const pilz_industrial_motion_planner::LimitsContainer& planner_limits);
+                         const pilz_industrial_motion_planner::LimitsContainer& planner_limits,
+                         const std::string& group_name);
 
 private:
   void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
@@ -78,9 +79,8 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator
    * @param sampling_time
    */
   void planPTP(const std::map<std::string, double>& start_pos, const std::map<std::string, double>& goal_pos,
-               trajectory_msgs::msg::JointTrajectory& joint_trajectory, const std::string& group_name,
-               const double& velocity_scaling_factor, const double& acceleration_scaling_factor,
-               const double& sampling_time);
+               trajectory_msgs::msg::JointTrajectory& joint_trajectory, const double& velocity_scaling_factor,
+               const double& acceleration_scaling_factor, const double& sampling_time);
 
   void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
             const MotionPlanInfo& plan_info, const double& sampling_time,
@@ -89,8 +89,8 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator
 private:
   const double MIN_MOVEMENT = 0.001;
   pilz_industrial_motion_planner::JointLimitsContainer joint_limits_;
-  // most strict joint limits for each group
-  std::map<std::string, JointLimit> most_strict_limits_;
+  // most strict joint limits
+  JointLimit most_strict_limit_;
 };
 
 }  // namespace pilz_industrial_motion_planner

moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp

@@ -116,7 +116,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updatePositionLimitF
   {
     // LCOV_EXCL_START
     case 0:
-      RCLCPP_ERROR_STREAM(LOGGER, "no bounds set for joint " << joint_model->getName());
+      RCLCPP_WARN_STREAM(LOGGER, "no bounds set for joint " << joint_model->getName());
       break;
     // LCOV_EXCL_STOP
     case 1:
@@ -126,7 +126,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updatePositionLimitF
       break;
     // LCOV_EXCL_START
     default:
-      RCLCPP_ERROR_STREAM(LOGGER, "Multi-DOF-Joints not supported. The robot won't move. " << joint_model->getName());
+      RCLCPP_WARN_STREAM(LOGGER, "Multi-DOF-Joint '" << joint_model->getName() << "' not supported.");
       joint_limit.has_position_limits = true;
       joint_limit.min_position = 0;
       joint_limit.max_position = 0;
@@ -145,7 +145,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updateVelocityLimitF
   {
     // LCOV_EXCL_START
     case 0:
-      RCLCPP_ERROR_STREAM(LOGGER, "no bounds set for joint " << joint_model->getName());
+      RCLCPP_WARN_STREAM(LOGGER, "no bounds set for joint " << joint_model->getName());
       break;
     // LCOV_EXCL_STOP
     case 1:
@@ -154,7 +154,7 @@ void pilz_industrial_motion_planner::JointLimitsAggregator::updateVelocityLimitF
       break;
     // LCOV_EXCL_START
     default:
-      RCLCPP_ERROR_STREAM(LOGGER, "Multi-DOF-Joints not supported. The robot won't move.");
+      RCLCPP_WARN_STREAM(LOGGER, "Multi-DOF-Joint '" << joint_model->getName() << "' not supported.");
       joint_limit.has_velocity_limits = true;
       joint_limit.max_velocity = 0;
       break;

moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp

@@ -133,8 +133,8 @@ CommandPlanner::getPlanningContext(const planning_scene::PlanningSceneConstPtr&
   // Check that a loaded for this request exists
   if (!canServiceRequest(req))
   {
-    RCLCPP_ERROR_STREAM(LOGGER, "No ContextLoader for planner_id " << req.planner_id.c_str()
-                                                                   << " found. Planning not possible.");
+    RCLCPP_ERROR_STREAM(LOGGER, "No ContextLoader for planner_id '" << req.planner_id.c_str()
+                                                                    << "' found. Planning not possible.");
     return nullptr;
   }
 

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp

@@ -59,7 +59,8 @@ namespace pilz_industrial_motion_planner
 static const rclcpp::Logger LOGGER =
     rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_circ");
 TrajectoryGeneratorCIRC::TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr& robot_model,
-                                                 const LimitsContainer& planner_limits)
+                                                 const LimitsContainer& planner_limits,
+                                                 const std::string& /*group_name*/)
   : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits)
 {
   if (!planner_limits_.hasFullCartesianLimits())

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp

@@ -60,7 +60,7 @@ namespace pilz_industrial_motion_planner
 static const rclcpp::Logger LOGGER =
     rclcpp::get_logger("moveit.pilz_industrial_motion_planner.trajectory_generator_lin");
 TrajectoryGeneratorLIN::TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr& robot_model,
-                                               const LimitsContainer& planner_limits)
+                                               const LimitsContainer& planner_limits, const std::string& /*group_name*/)
   : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits)
 {
   if (!planner_limits_.hasFullCartesianLimits())