Ports moveit/moveit/pull/3519 to ros2

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);
 
 /**

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h

@@ -48,7 +48,6 @@ namespace pilz_industrial_motion_planner
 CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinTrajectoryConversionFailure, moveit_msgs::msg::MoveItErrorCodes::FAILURE);
 
 CREATE_MOVEIT_ERROR_CODE_EXCEPTION(JointNumberMismatch, moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
-CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinJointMissingInStartState, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
 CREATE_MOVEIT_ERROR_CODE_EXCEPTION(LinInverseForGoalIncalculable, moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION);
 
 /**

moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp

@@ -100,7 +100,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend(
 
   if (!generateJointTrajectory(planning_scene, limits_.getJointLimitContainer(), blend_trajectory_cartesian,
                                req.group_name, req.link_name, initial_joint_position, initial_joint_velocity,
-                               blend_joint_trajectory, error_code, true))
+                               blend_joint_trajectory, error_code))
   {
     // LCOV_EXCL_START
     RCLCPP_INFO(getLogger(), "Failed to generate joint trajectory for blending trajectory.");

moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp

@@ -63,13 +63,6 @@ bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::Plannin
     return false;
   }
 
-  if (!robot_model->getJointModelGroup(group_name)->canSetStateFromIK(link_name))
-  {
-    RCLCPP_ERROR_STREAM(getLogger(),
-                        "No valid IK solver exists for " << link_name << " in planning group " << group_name);
-    return false;
-  }
-
   if (frame_id != robot_model->getModelFrame())
   {
     RCLCPP_ERROR_STREAM(getLogger(), "Given frame (" << frame_id << ") is unequal to model frame("
@@ -81,18 +74,22 @@ bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::Plannin
   rstate.setVariablePositions(seed);
 
   moveit::core::GroupStateValidityCallbackFn ik_constraint_function;
-  ik_constraint_function = [check_self_collision, scene](moveit::core::RobotState* robot_state,
-                                                         const moveit::core::JointModelGroup* joint_group,
-                                                         const double* joint_group_variable_values) {
-    return pilz_industrial_motion_planner::isStateColliding(check_self_collision, scene, robot_state, joint_group,
-                                                            joint_group_variable_values);
-  };
+  if (check_self_collision)
+  {
+    ik_constraint_function = [scene](moveit::core::RobotState* robot_state,
+                                     const moveit::core::JointModelGroup* joint_group,
+                                     const double* joint_group_variable_values) {
+      return pilz_industrial_motion_planner::isStateColliding(scene, robot_state, joint_group,
+                                                              joint_group_variable_values);
+    };
+  }
 
   // call ik
-  if (rstate.setFromIK(robot_model->getJointModelGroup(group_name), pose, link_name, timeout, ik_constraint_function))
+  const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group_name);
+  if (rstate.setFromIK(jmg, pose, link_name, timeout, ik_constraint_function))
   {
     // copy the solution
-    for (const auto& joint_name : robot_model->getJointModelGroup(group_name)->getActiveJointModelNames())
+    for (const auto& joint_name : jmg->getActiveJointModelNames())
     {
       solution[joint_name] = rstate.getVariablePosition(joint_name);
     }
@@ -120,25 +117,22 @@ bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::Plannin
                        timeout);
 }
 
-bool pilz_industrial_motion_planner::computeLinkFK(const planning_scene::PlanningSceneConstPtr& scene,
-                                                   const std::string& link_name,
+bool pilz_industrial_motion_planner::computeLinkFK(moveit::core::RobotState& robot_state, const std::string& link_name,
                                                    const std::map<std::string, double>& joint_state,
                                                    Eigen::Isometry3d& pose)
-{  // take robot state from the current scene
-  moveit::core::RobotState rstate{ scene->getCurrentState() };
-
+{
   // check the reference frame of the target pose
-  if (!rstate.knowsFrameTransform(link_name))
+  if (!robot_state.knowsFrameTransform(link_name))
   {
     RCLCPP_ERROR_STREAM(getLogger(), "The target link " << link_name << " is not known by robot.");
     return false;
   }
 
-  rstate.setVariablePositions(joint_state);
+  robot_state.setVariablePositions(joint_state);
 
   // update the frame
-  rstate.update();
-  pose = rstate.getFrameTransform(link_name);
+  robot_state.update();
+  pose = robot_state.getFrameTransform(link_name);
 
   return true;
 }
@@ -574,17 +568,11 @@ bool pilz_industrial_motion_planner::intersectionFound(const Eigen::Vector3d& p_
   return ((p_current - p_center).norm() <= r) && ((p_next - p_center).norm() >= r);
 }
 
-bool pilz_industrial_motion_planner::isStateColliding(const bool test_for_self_collision,
-                                                      const planning_scene::PlanningSceneConstPtr& scene,
+bool pilz_industrial_motion_planner::isStateColliding(const planning_scene::PlanningSceneConstPtr& scene,
                                                       moveit::core::RobotState* rstate,
                                                       const moveit::core::JointModelGroup* const group,
                                                       const double* const ik_solution)
 {
-  if (!test_for_self_collision)
-  {
-    return true;
-  }
-
   rstate->setJointGroupPositions(group, ik_solution);
   rstate->update();
   collision_detection::CollisionRequest collision_req;

moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp

@@ -121,11 +121,6 @@ void TrajectoryGenerator::checkForValidGroupName(const std::string& group_name)
 void TrajectoryGenerator::checkStartState(const moveit_msgs::msg::RobotState& start_state,
                                           const std::string& group) const
 {
-  if (start_state.joint_state.name.empty())
-  {
-    throw NoJointNamesInStartState("No joint names for state state given");
-  }
-
   if (start_state.joint_state.name.size() != start_state.joint_state.position.size())
   {
     throw SizeMismatchInStartState("Joint state name and position do not match in start state");
@@ -158,19 +153,11 @@ void TrajectoryGenerator::checkStartState(const moveit_msgs::msg::RobotState& st
 }
 
 void TrajectoryGenerator::checkJointGoalConstraint(const moveit_msgs::msg::Constraints& constraint,
-                                                   const std::vector<std::string>& expected_joint_names,
                                                    const std::string& group_name) const
 {
   for (const auto& joint_constraint : constraint.joint_constraints)
   {
     const std::string& curr_joint_name{ joint_constraint.joint_name };
-    if (std::find(expected_joint_names.cbegin(), expected_joint_names.cend(), curr_joint_name) ==
-        expected_joint_names.cend())
-    {
-      std::ostringstream os;
-      os << "Cannot find joint \"" << curr_joint_name << "\" from start state in goal constraint";
-      throw StartStateGoalStateMismatch(os.str());
-    }
 
     if (!robot_model_->getJointModelGroup(group_name)->hasJointModel(curr_joint_name))
     {
@@ -189,7 +176,8 @@ void TrajectoryGenerator::checkJointGoalConstraint(const moveit_msgs::msg::Const
 }
 
 void TrajectoryGenerator::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
 {
   assert(constraint.position_constraints.size() == 1);
   assert(constraint.orientation_constraints.size() == 1);
@@ -215,7 +203,8 @@ void TrajectoryGenerator::checkCartesianGoalConstraint(const moveit_msgs::msg::C
     throw PositionOrientationConstraintNameMismatch(os.str());
   }
 
-  if (!robot_model_->getJointModelGroup(group_name)->canSetStateFromIK(pos_constraint.link_name))
+  const auto& lm = robot_state.getRigidlyConnectedParentLinkModel(pos_constraint.link_name);
+  if (!lm || !jmg->canSetStateFromIK(lm->getName()))
   {
     std::ostringstream os;
     os << "No IK solver available for link: \"" << pos_constraint.link_name << '\"';
@@ -229,8 +218,8 @@ void TrajectoryGenerator::checkCartesianGoalConstraint(const moveit_msgs::msg::C
 }
 
 void TrajectoryGenerator::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 moveit_msgs::msg::MotionPlanRequest::_goal_constraints_type& goal_constraints, const std::string& group_name,
+    const moveit::core::RobotState& rstate) const
 {
   if (goal_constraints.size() != 1)
   {
@@ -247,21 +236,22 @@ void TrajectoryGenerator::checkGoalConstraints(
 
   if (isJointGoalGiven(goal_con))
   {
-    checkJointGoalConstraint(goal_con, expected_joint_names, group_name);
+    checkJointGoalConstraint(goal_con, group_name);
   }
   else
   {
-    checkCartesianGoalConstraint(goal_con, group_name);
+    checkCartesianGoalConstraint(goal_con, rstate, robot_model_->getJointModelGroup(group_name));
   }
 }
 
-void TrajectoryGenerator::validateRequest(const planning_interface::MotionPlanRequest& req) const
+void TrajectoryGenerator::validateRequest(const planning_interface::MotionPlanRequest& req,
+                                          const moveit::core::RobotState& rstate) const
 {
   checkVelocityScaling(req.max_velocity_scaling_factor);
   checkAccelerationScaling(req.max_acceleration_scaling_factor);
   checkForValidGroupName(req.group_name);
   checkStartState(req.start_state, req.group_name);
-  checkGoalConstraints(req.goal_constraints, req.start_state.joint_state.name, req.group_name);
+  checkGoalConstraints(req.goal_constraints, req.group_name, rstate);
 }
 
 void TrajectoryGenerator::setSuccessResponse(const moveit::core::RobotState& start_state, const std::string& group_name,
@@ -317,7 +307,7 @@ void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr&
   res.planner_id = req.planner_id;
   try
   {
-    validateRequest(req);
+    validateRequest(req, scene->getCurrentState());
   }
   catch (const MoveItErrorCodeException& ex)
   {
@@ -339,7 +329,7 @@ void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr&
     return;
   }
 
-  MotionPlanInfo plan_info;
+  MotionPlanInfo plan_info(scene, req);
   try
   {
     extractMotionPlanInfo(scene, req, plan_info);
@@ -355,7 +345,7 @@ void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr&
   trajectory_msgs::msg::JointTrajectory joint_trajectory;
   try
   {
-    plan(scene, req, plan_info, sampling_time, joint_trajectory);
+    plan(plan_info.start_scene, req, plan_info, sampling_time, joint_trajectory);
   }
   catch (const MoveItErrorCodeException& ex)
   {
@@ -365,9 +355,29 @@ void TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr&
     return;
   }
 
-  moveit::core::RobotState start_state(scene->getCurrentState());
-  moveit::core::robotStateMsgToRobotState(req.start_state, start_state, true);
-  setSuccessResponse(start_state, req.group_name, joint_trajectory, planning_begin, res);
+  setSuccessResponse(plan_info.start_scene->getCurrentState(), req.group_name, joint_trajectory, planning_begin, res);
+}
+
+TrajectoryGenerator::MotionPlanInfo::MotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
+                                                    const planning_interface::MotionPlanRequest& req)
+{
+  auto ps = scene->diff();
+  auto& start_state = ps->getCurrentStateNonConst();
+  // update start state from req
+  moveit::core::robotStateMsgToRobotState(scene->getTransforms(), req.start_state, start_state);
+  start_state.update();
+  start_scene = std::move(ps);
+
+  // initialize info.start_joint_position with active joint values from start_state
+  const double* positions = start_state.getVariablePositions();
+  for (const auto* jm : start_state.getRobotModel()->getJointModelGroup(req.group_name)->getActiveJointModels())
+  {
+    const auto& names = jm->getVariableNames();
+    for (std::size_t i = 0, j = jm->getFirstVariableIndex(); i < jm->getVariableCount(); ++i, ++j)
+    {
+      start_joint_position[names[i]] = positions[j];
+    }
+  }
 }
 
 }  // namespace pilz_industrial_motion_planner