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

.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-*-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 && \

moveit_core/constraint_samplers/src/default_constraint_samplers.cpp

@@ -515,21 +515,20 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q
              moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR)
     {
       Eigen::Vector3d rotation_vector(angle_x, angle_y, angle_z);
+      // convert rotation vector from frame_id to target frame
+      rotation_vector =
+          sampling_pose_.orientation_constraint_->getDesiredRotationMatrixInRefFrame().transpose() * rotation_vector;
       diff = Eigen::Isometry3d(Eigen::AngleAxisd(rotation_vector.norm(), rotation_vector.normalized()));
     }
     else
     {
       /* The parameterization type should be validated in configure, so this should never happen. */
       RCLCPP_ERROR(getLogger(), "The parameterization type for the orientation constraints is invalid.");
     }
-    // diff is isometry by construction
-    // getDesiredRotationMatrix() returns a valid rotation matrix by contract
-    // reqr has thus to be a valid isometry
-    Eigen::Isometry3d reqr(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.linear());
-    quat = Eigen::Quaterniond(reqr.linear());  // reqr is isometry, so quat has to be normalized
 
-    // if this constraint is with respect a mobile frame, we need to convert this rotation to the root frame of the
-    // model
+    quat = Eigen::Quaterniond(sampling_pose_.orientation_constraint_->getDesiredRotationMatrix() * diff.linear());
+
+    // if this constraint is with respect to a mobile frame, we need to convert this rotation to the root frame of the model
     if (sampling_pose_.orientation_constraint_->mobileReferenceFrame())
     {
       // getFrameTransform() returns a valid isometry by contract

moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h

@@ -336,12 +336,19 @@ MOVEIT_CLASS_FORWARD(OrientationConstraint);  // Defines OrientationConstraintPt
 /**
  * \brief Class for constraints on the orientation of a link
  *
- * This class expresses an orientation constraint on a particular
- * link.  The constraint is specified in terms of a quaternion, with
- * tolerances on X,Y, and Z axes.  The rotation difference is computed
- * based on the XYZ Euler angle formulation (intrinsic rotations) or as a rotation vector. This depends on the
- * `Parameterization` type. The header on the quaternion can be specified in terms of either a fixed or a mobile
- * frame.  The type value will return ORIENTATION_CONSTRAINT.
+ * This class expresses an orientation constraint on a particular link.
+ * The constraint specifies a target orientation via a quaternion as well as
+ * tolerances on X,Y, and Z rotation axes.
+ * The rotation difference between the target and actual link orientation is expressed
+ * either as XYZ Euler angles or as a rotation vector (depending on the `parameterization` type).
+ * The latter is highly recommended, because it supports resolution of subframes and attached bodies.
+ * Also, rotation vector representation allows to interpret the tolerances always w.r.t. the given reference frame.
+ * Euler angles are much more restricted and exhibit singularities.
+ *
+ * For efficiency, if the target orientation is expressed w.r.t. to a fixed frame (relative to the planning frame),
+ * some stuff is precomputed. However, mobile reference frames are supported as well.
+ *
+ * The type value will return ORIENTATION_CONSTRAINT.
  *
  */
 class OrientationConstraint : public KinematicConstraint
@@ -439,6 +446,19 @@ class OrientationConstraint : public KinematicConstraint
    *
    * The returned matrix is always a valid rotation matrix.
    */
+  const Eigen::Matrix3d& getDesiredRotationMatrixInRefFrame() const
+  {
+    // validity of the rotation matrix is enforced in configure()
+    return desired_R_in_frame_id_;
+  }
+
+  /**
+   * \brief The rotation target in the reference or tf frame.
+   *
+   * @return The target rotation.
+   *
+   * The returned matrix is always a valid rotation matrix.
+   */
   const Eigen::Matrix3d& getDesiredRotationMatrix() const
   {
     // validity of the rotation matrix is enforced in configure()
@@ -484,16 +504,15 @@ class OrientationConstraint : public KinematicConstraint
   }
 
 protected:
-  const moveit::core::LinkModel* link_model_;   /**< \brief The target link model */
-  Eigen::Matrix3d desired_rotation_matrix_;     /**< \brief The desired rotation matrix in the tf frame. Guaranteed to
-                                                 * be valid rotation matrix. */
-  Eigen::Matrix3d desired_rotation_matrix_inv_; /**< \brief The inverse of the desired rotation matrix, precomputed for
-                                                 * efficiency. Guaranteed to be valid rotation matrix. */
-  std::string desired_rotation_frame_id_;       /**< \brief The target frame of the transform tree */
-  bool mobile_frame_;                           /**< \brief Whether or not the header frame is mobile or fixed */
-  int parameterization_type_;                   /**< \brief Parameterization type for orientation tolerance. */
+  const moveit::core::LinkModel* link_model_;   /**< The target link model */
+  Eigen::Matrix3d desired_R_in_frame_id_;       /**< Desired rotation matrix in frame_id */
+  Eigen::Matrix3d desired_rotation_matrix_;     /**< The desired rotation matrix in the tf frame */
+  Eigen::Matrix3d desired_rotation_matrix_inv_; /**< The inverse of desired_rotation_matrix_ (for efficiency) */
+  std::string desired_rotation_frame_id_;       /**< The target frame of the transform tree */
+  bool mobile_frame_;                           /**< Whether or not the header frame is mobile or fixed */
+  int parameterization_type_;                   /**< Parameterization type for orientation tolerance */
   double absolute_x_axis_tolerance_, absolute_y_axis_tolerance_,
-      absolute_z_axis_tolerance_; /**< \brief Storage for the tolerances */
+      absolute_z_axis_tolerance_; /**< Storage for the tolerances */
 };
 
 MOVEIT_CLASS_FORWARD(PositionConstraint);  // Defines PositionConstraintPtr, ConstPtr, WeakPtr... etc

moveit_core/kinematic_constraints/src/kinematic_constraint.cpp

@@ -597,7 +597,8 @@ bool OrientationConstraint::configure(const moveit_msgs::msg::OrientationConstra
   // clearing out any old data
   clear();
 
-  link_model_ = robot_model_->getLinkModel(oc.link_name);
+  bool found;  // just needed to silent the error message in getLinkModel()
+  link_model_ = robot_model_->getLinkModel(oc.link_name, &found);
   if (!link_model_)
   {
     RCLCPP_WARN(getLogger(), "Could not find link model for link name %s", oc.link_name.c_str());
@@ -617,6 +618,7 @@ bool OrientationConstraint::configure(const moveit_msgs::msg::OrientationConstra
   if (oc.header.frame_id.empty())
     RCLCPP_WARN(getLogger(), "No frame specified for position constraint on link '%s'!", oc.link_name.c_str());
 
+  desired_R_in_frame_id_ = Eigen::Quaterniond(q);  // desired rotation wrt. frame_id
   if (tf.isFixedFrame(oc.header.frame_id))
   {
     tf.transformQuaternion(oc.header.frame_id, q, q);
@@ -749,10 +751,8 @@ ConstraintEvaluationResult OrientationConstraint::decide(const moveit::core::Rob
   else if (parameterization_type_ == moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR)
   {
     Eigen::AngleAxisd aa(diff.linear());
-    xyz_rotation = aa.axis() * aa.angle();
-    xyz_rotation(0) = fabs(xyz_rotation(0));
-    xyz_rotation(1) = fabs(xyz_rotation(1));
-    xyz_rotation(2) = fabs(xyz_rotation(2));
+    // transform rotation vector from target frame to frame_id and take absolute values
+    xyz_rotation = (desired_R_in_frame_id_ * (aa.axis() * aa.angle())).cwiseAbs();
   }
   else
   {

moveit_core/kinematic_constraints/src/utils.cpp

@@ -217,6 +217,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n
 
   goal.orientation_constraints.resize(1);
   moveit_msgs::msg::OrientationConstraint& ocm = goal.orientation_constraints[0];
+  ocm.parameterization = moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR;
   ocm.link_name = link_name;
   ocm.header = pose.header;
   ocm.orientation = pose.pose.orientation;
@@ -655,9 +656,17 @@ bool resolveConstraintFrames(const moveit::core::RobotState& state, moveit_msgs:
     // the constraint needs to be expressed in the frame of a robot link.
     if (c.link_name != robot_link->getName())
     {
+      if (c.parameterization == moveit_msgs::msg::OrientationConstraint::XYZ_EULER_ANGLES)
+      {
+        RCLCPP_ERROR(getLogger(),
+                     "Euler angles parameterization is not supported for non-link frames in orientation constraints. \n"
+                     "Switch to rotation vector parameterization.");
+        return false;
+      }
       c.link_name = robot_link->getName();
       Eigen::Quaterniond link_name_to_robot_link(transform.linear().transpose() *
                                                  state.getGlobalLinkTransform(robot_link).linear());
+      // adapt goal orientation
       Eigen::Quaterniond quat_target;
       tf2::fromMsg(c.orientation, quat_target);
       c.orientation = tf2::toMsg(quat_target * link_name_to_robot_link);

moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp

@@ -475,6 +475,42 @@ TEST_F(FloatingJointRobot, OrientationConstraintsParameterization)
   EXPECT_FALSE(oc_rotvec.decide(robot_state).satisfied);
 }
 
+TEST_F(FloatingJointRobot, ToleranceInRefFrame)
+{
+  moveit::core::RobotState robot_state(robot_model_);
+  robot_state.setToDefaultValues();
+  auto base = rotationVectorToQuaternion(M_PI / 2.0, 0, 0);  // base rotation: 90° about x
+  setRobotEndEffectorOrientation(robot_state, base);
+  robot_state.update();
+
+  moveit::core::Transforms tf(robot_model_->getModelFrame());
+
+  // create message to configure orientation constraints
+  moveit_msgs::msg::OrientationConstraint ocm;
+  ocm.link_name = "ee";
+  ocm.header.frame_id = robot_model_->getModelFrame();
+  ocm.parameterization = moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR;
+  ocm.orientation = tf2::toMsg(base);
+  ocm.absolute_x_axis_tolerance = 0.2;
+  ocm.absolute_y_axis_tolerance = 0.2;
+  ocm.absolute_z_axis_tolerance = 1.0;
+  ocm.weight = 1.0;
+
+  kinematic_constraints::OrientationConstraint oc(robot_model_);
+  EXPECT_TRUE(oc.configure(ocm, tf));
+
+  EXPECT_TRUE(oc.decide(robot_state).satisfied);  // link and target are perfectly aligned
+
+  // strong rotation w.r.t. base frame is ok
+  auto delta = rotationVectorToQuaternion(0.1, 0.1, 0.9);
+  setRobotEndEffectorOrientation(robot_state, delta * base);
+  EXPECT_TRUE(oc.decide(robot_state).satisfied);
+
+  // strong rotation w.r.t. link frame is not ok
+  setRobotEndEffectorOrientation(robot_state, base * delta);
+  EXPECT_FALSE(oc.decide(robot_state).satisfied);  // link and target are perfectly aligned
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);

moveit_core/robot_model/include/moveit/robot_model/joint_model.h

@@ -514,10 +514,10 @@ class JointModel
   /** \brief The joint this one mimics (nullptr for joints that do not mimic) */
   const JointModel* mimic_;
 
-  /** \brief The offset to the mimic joint */
+  /** \brief The multiplier to the mimic joint */
   double mimic_factor_;
 
-  /** \brief The multiplier to the mimic joint */
+  /** \brief The offset to the mimic joint */
   double mimic_offset_;
 
   /** \brief The set of joints that should get a value copied to them when this joint changes */

moveit_core/trajectory_processing/src/trajectory_tools.cpp

@@ -87,7 +87,7 @@ trajectory_msgs::msg::JointTrajectory createTrajectoryMessage(const std::vector<
   }
   trajectory_msg.joint_names = joint_names;
   const double time_step = 1.0 / static_cast<double>(sampling_rate);
-  const int n_samples = static_cast<int>(trajectory.getDuration() / time_step) + 1;
+  const int n_samples = static_cast<int>(std::ceil(trajectory.getDuration() / time_step)) + 1;
   trajectory_msg.points.reserve(n_samples);
   for (int sample = 0; sample < n_samples; ++sample)
   {

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h

@@ -145,5 +145,6 @@ class PoseModelStateSpace : public ModelBasedStateSpace
   };
 
   std::vector<PoseComponent> poses_;
+  double jump_factor_;
 };
 }  // namespace ompl_interface

moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp

@@ -54,6 +54,8 @@ const std::string PoseModelStateSpace::PARAMETERIZATION_TYPE = "PoseModel";
 
 PoseModelStateSpace::PoseModelStateSpace(const ModelBasedStateSpaceSpecification& spec) : ModelBasedStateSpace(spec)
 {
+  jump_factor_ = 1.5;  // \todo make this a param
+
   if (spec.joint_model_group_->getGroupKinematics().first)
   {
     poses_.emplace_back(spec.joint_model_group_, spec.joint_model_group_->getGroupKinematics().first);
@@ -131,9 +133,31 @@ void PoseModelStateSpace::sanityChecks() const
 void PoseModelStateSpace::interpolate(const ompl::base::State* from, const ompl::base::State* to, const double t,
                                       ompl::base::State* state) const
 {
-  // interpolate in joint space
+  // we want to interpolate in Cartesian space to avoid rejection of path constraints
+
+  // interpolate in joint space to find a suitable seed for IK
   ModelBasedStateSpace::interpolate(from, to, t, state);
-  computeStateFK(state);
+  double d_joint = ModelBasedStateSpace::distance(from, state);
+
+  // interpolate SE3 components
+  for (std::size_t i = 0; i < poses_.size(); ++i)
+  {
+    poses_[i].state_space_->interpolate(from->as<StateType>()->poses[i], to->as<StateType>()->poses[i], t,
+                                        state->as<StateType>()->poses[i]);
+  }
+
+  // the call above may reset all flags for state; but we know the pose we want flag should be set
+  state->as<StateType>()->setPoseComputed(true);
+
+  // compute IK for interpolated Cartesian state
+  if (computeStateIK(state))
+  {
+    double d_cart = ModelBasedStateSpace::distance(from, state);
+
+    // reject if Cartesian interpolation yields much larger distance than joint interpolation
+    if (d_cart > jump_factor_ * d_joint)
+      state->as<StateType>()->markInvalid();
+  }
 }
 
 void PoseModelStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ)

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h

@@ -126,13 +126,6 @@ void pilz_industrial_motion_planner::PlanningContextBase<GeneratorT>::solve(plan
     res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED;
     return;
   }
-  // Use current state as start state if not set
-  if (request_.start_state.joint_state.name.empty())
-  {
-    moveit_msgs::msg::RobotState current_state;
-    moveit::core::robotStateToRobotStateMsg(getPlanningScene()->getCurrentState(), current_state);
-    request_.start_state = current_state;
-  }
   generator_.generate(getPlanningScene(), request_, res);
 }
 

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h

@@ -45,6 +45,7 @@
 
 #include <pilz_industrial_motion_planner/cartesian_trajectory.h>
 #include <pilz_industrial_motion_planner/limits_container.h>
+#include <pilz_industrial_motion_planner/trajectory_generation_exceptions.h>
 
 namespace pilz_industrial_motion_planner
 {
@@ -74,17 +75,17 @@ bool computePoseIK(const planning_scene::PlanningSceneConstPtr& scene, const std
                    bool check_self_collision = true, const double timeout = 0.0);
 
 /**
- * @brief compute the pose of a link at give robot state
- * @param robot_model: kinematic model of the robot
+ * @brief compute the pose of a link at a given robot state
+ * @param robot_state: an arbitrary robot state (with collision objects attached)
  * @param link_name: target link name
  * @param joint_state: joint positions of this group
  * @param pose: pose of the link in base frame of robot model
  * @return true if succeed
  */
-bool computeLinkFK(const planning_scene::PlanningSceneConstPtr& scene, const std::string& link_name,
+bool computeLinkFK(moveit::core::RobotState& robot_state, const std::string& link_name,
                    const std::map<std::string, double>& joint_state, Eigen::Isometry3d& pose);
 
-bool computeLinkFK(const planning_scene::PlanningSceneConstPtr& scene, const std::string& link_name,
+bool computeLinkFK(moveit::core::RobotState& robot_state, const std::string& link_name,
                    const std::vector<std::string>& joint_names, const std::vector<double>& joint_positions,
                    Eigen::Isometry3d& pose);
 
@@ -212,9 +213,8 @@ bool intersectionFound(const Eigen::Vector3d& p_center, const Eigen::Vector3d& p
  * @param ik_solution
  * @return
  */
-bool isStateColliding(const bool test_for_self_collision, const planning_scene::PlanningSceneConstPtr& scene,
-                      moveit::core::RobotState* state, const moveit::core::JointModelGroup* const group,
-                      const double* const ik_solution);
+bool isStateColliding(const planning_scene::PlanningSceneConstPtr& scene, moveit::core::RobotState* state,
+                      const moveit::core::JointModelGroup* const group, const double* const ik_solution);
 }  // namespace pilz_industrial_motion_planner
 
 void normalizeQuaternion(geometry_msgs::msg::Quaternion& quat);

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h

@@ -111,19 +111,21 @@ class TrajectoryGenerator
 
 protected:
   /**
-   * @brief This class is used to extract needed information from motion plan
-   * request.
+   * @brief This class is used to extract needed information from motion plan request.
    */
   class MotionPlanInfo
   {
   public:
+    MotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req);
+
     std::string group_name;
     std::string link_name;
     Eigen::Isometry3d start_pose;
     Eigen::Isometry3d goal_pose;
     std::map<std::string, double> start_joint_position;
     std::map<std::string, double> goal_joint_position;
     std::pair<std::string, Eigen::Vector3d> circ_path_point;
+    planning_scene::PlanningSceneConstPtr start_scene;  // scene with updated start state
   };
 
   /**
@@ -199,7 +201,7 @@ class TrajectoryGenerator
    * moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS on failure
    * @param req: motion plan request
    */
-  void validateRequest(const planning_interface::MotionPlanRequest& req) const;
+  void validateRequest(const planning_interface::MotionPlanRequest& req, const moveit::core::RobotState& rstate) const;
 
   /**
    * @brief set MotionPlanResponse from joint trajectory
@@ -226,14 +228,13 @@ class TrajectoryGenerator
   void checkStartState(const moveit_msgs::msg::RobotState& start_state, const std::string& group) const;
 
   void checkGoalConstraints(const moveit_msgs::msg::MotionPlanRequest::_goal_constraints_type& goal_constraints,
-                            const std::vector<std::string>& expected_joint_names, const std::string& group_name) const;
+                            const std::string& group_name, const moveit::core::RobotState& rstate) const;
 
-  void checkJointGoalConstraint(const moveit_msgs::msg::Constraints& constraint,
-                                const std::vector<std::string>& expected_joint_names,
-                                const std::string& group_name) const;
+  void checkJointGoalConstraint(const moveit_msgs::msg::Constraints& constraint, const std::string& group_name) const;
 
   void checkCartesianGoalConstraint(const moveit_msgs::msg::Constraints& constraint,
-                                    const std::string& group_name) const;
+                                    const moveit::core::RobotState& robot_state,
+                                    const moveit::core::JointModelGroup* const jmg) const;
 
 private:
   /**

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h

@@ -56,8 +56,6 @@ CREATE_MOVEIT_ERROR_CODE_EXCEPTION(UnknownLinkNameOfAuxiliaryPoint,
                                    moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME);
 CREATE_MOVEIT_ERROR_CODE_EXCEPTION(NumberOfConstraintsMismatch,
                                    moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
-CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircJointMissingInStartState,
-                                   moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
 CREATE_MOVEIT_ERROR_CODE_EXCEPTION(CircInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION);
 
 /**