From 70e1aae8bd2ee58baf136db74ca3b831186ec8fe Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Tue, 5 Nov 2024 11:14:25 +0000 Subject: [PATCH] Ports moveit/moveit/pull/3519 to ros2 (#3055) * Ports moveit/moveit/pull/3519 to ros2 * Applies formatting * Fixes compilation errors * Fixes compilation errors * Fixes compilation errors * Fixes compilation errors * Replaces RobotState with pointer in tests * Fixes tests --- .../planning_context_base.h | 7 -- .../trajectory_functions.h | 14 ++-- .../trajectory_generator.h | 17 ++--- .../trajectory_generator_circ.h | 2 - .../trajectory_generator_lin.h | 1 - .../trajectory_blender_transition_window.cpp | 2 +- .../src/trajectory_functions.cpp | 50 ++++++--------- .../src/trajectory_generator.cpp | 64 +++++++++++-------- .../src/trajectory_generator_circ.cpp | 20 +----- .../src/trajectory_generator_lin.cpp | 24 +------ .../src/trajectory_generator_ptp.cpp | 7 -- .../src/unittest_trajectory_functions.cpp | 10 +-- .../unittest_trajectory_generator_circ.cpp | 23 ------- .../src/unittest_trajectory_generator_lin.cpp | 23 ------- 14 files changed, 85 insertions(+), 179 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h index 449bd3dc8a..7597316717 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h +++ b/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::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); } diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h index 1dd242b703..0adf6d1385 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h @@ -45,6 +45,7 @@ #include #include +#include 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& 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& joint_names, const std::vector& 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); diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h index 48c77f53db..69debc2b98 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h @@ -111,12 +111,13 @@ 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; @@ -124,6 +125,7 @@ class TrajectoryGenerator std::map start_joint_position; std::map goal_joint_position; std::pair 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& 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& 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: /** diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h index 2246b2c3d6..ef4ab6c814 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h +++ b/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); /** diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h index 19e48dac38..61592e0794 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h +++ b/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); /** diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp index ebdc3d3ee7..84e6679709 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp +++ b/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."); diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 555421a685..7a4ea937bf 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/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& 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; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp index dfeeec3f55..4b0482ca64 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/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& 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& 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 diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index c512e21578..ae1e7b9b10 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -98,6 +98,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni info.group_name = req.group_name; std::string frame_id{ robot_model_->getModelFrame() }; + moveit::core::RobotState robot_state = scene->getCurrentState(); // goal given in joint space if (!req.goal_constraints.front().joint_constraints.empty()) @@ -124,7 +125,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni info.goal_joint_position[joint_item.joint_name] = joint_item.position; } - computeLinkFK(scene, info.link_name, info.goal_joint_position, info.goal_pose); + computeLinkFK(robot_state, info.link_name, info.goal_joint_position, info.goal_pose); } // goal given in Cartesian space else @@ -144,22 +145,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni info.goal_pose = getConstraintPose(req.goal_constraints.front()); } - assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); - for (const auto& joint_name : robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames()) - { - auto it{ std::find(req.start_state.joint_state.name.cbegin(), req.start_state.joint_state.name.cend(), joint_name) }; - if (it == req.start_state.joint_state.name.cend()) - { - std::ostringstream os; - os << "Could not find joint \"" << joint_name << "\" of group \"" << req.group_name - << "\" in start state of request"; - throw CircJointMissingInStartState(os.str()); - } - size_t index = it - req.start_state.joint_state.name.cbegin(); - info.start_joint_position[joint_name] = req.start_state.joint_state.position[index]; - } - - computeLinkFK(scene, info.link_name, info.start_joint_position, info.start_pose); + computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose); // check goal pose ik before Cartesian motion plan starts std::map ik_solution; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index 07b222f915..43e5235e0c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -75,6 +75,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin info.group_name = req.group_name; std::string frame_id{ robot_model_->getModelFrame() }; + moveit::core::RobotState robot_state = scene->getCurrentState(); // goal given in joint space if (!req.goal_constraints.front().joint_constraints.empty()) @@ -97,9 +98,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin info.goal_joint_position[joint_item.joint_name] = joint_item.position; } - // Ignored return value because at this point the function should always - // return 'true'. - computeLinkFK(scene, info.link_name, info.goal_joint_position, info.goal_pose); + computeLinkFK(robot_state, info.link_name, info.goal_joint_position, info.goal_pose); } // goal given in Cartesian space else @@ -119,24 +118,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin info.goal_pose = getConstraintPose(req.goal_constraints.front()); } - assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size()); - for (const auto& joint_name : robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames()) - { - auto it{ std::find(req.start_state.joint_state.name.cbegin(), req.start_state.joint_state.name.cend(), joint_name) }; - if (it == req.start_state.joint_state.name.cend()) - { - std::ostringstream os; - os << "Could not find joint \"" << joint_name << "\" of group \"" << req.group_name - << "\" in start state of request"; - throw LinJointMissingInStartState(os.str()); - } - size_t index = it - req.start_state.joint_state.name.cbegin(); - info.start_joint_position[joint_name] = req.start_state.joint_state.position[index]; - } - - // Ignored return value because at this point the function should always - // return 'true'. - computeLinkFK(scene, info.link_name, info.start_joint_position, info.start_pose); + computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose); // check goal pose ik before Cartesian motion plan starts std::map ik_solution; diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp index ddea11c25a..b12f64224c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp @@ -221,13 +221,6 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin { info.group_name = req.group_name; - // extract start state information - info.start_joint_position.clear(); - for (std::size_t i = 0; i < req.start_state.joint_state.name.size(); ++i) - { - info.start_joint_position[req.start_state.joint_state.name[i]] = req.start_state.joint_state.position[i]; - } - // extract goal info.goal_joint_position.clear(); if (!req.goal_constraints.at(0).joint_constraints.empty()) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp index e6c0675cb5..b13b1c5606 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp @@ -95,6 +95,7 @@ class TrajectoryFunctionsTestBase : public testing::Test rm_loader_ = std::make_unique(node_); robot_model_ = rm_loader_->getModel(); ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model"; + robot_state_ = std::make_shared(robot_model_); planning_scene_ = std::make_shared(robot_model_); // get parameters @@ -138,6 +139,7 @@ class TrajectoryFunctionsTestBase : public testing::Test // ros stuff rclcpp::Node::SharedPtr node_; moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotStatePtr robot_state_; std::unique_ptr rm_loader_; planning_scene::PlanningSceneConstPtr planning_scene_; @@ -189,27 +191,27 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, TipLinkFK) { Eigen::Isometry3d tip_pose; std::map test_state = zero_state_; - EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(planning_scene_, group_tip_link_, test_state, tip_pose)); + EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(*robot_state_, group_tip_link_, test_state, tip_pose)); EXPECT_NEAR(tip_pose(0, 3), 0, EPSILON); EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON); EXPECT_NEAR(tip_pose(2, 3), L0 + L1 + L2 + L3, EPSILON); test_state[joint_names_.at(1)] = M_PI_2; - EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(planning_scene_, group_tip_link_, test_state, tip_pose)); + EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(*robot_state_, group_tip_link_, test_state, tip_pose)); EXPECT_NEAR(tip_pose(0, 3), L1 + L2 + L3, EPSILON); EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON); EXPECT_NEAR(tip_pose(2, 3), L0, EPSILON); test_state[joint_names_.at(1)] = -M_PI_2; test_state[joint_names_.at(2)] = M_PI_2; - EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(planning_scene_, group_tip_link_, test_state, tip_pose)); + EXPECT_TRUE(pilz_industrial_motion_planner::computeLinkFK(*robot_state_, group_tip_link_, test_state, tip_pose)); EXPECT_NEAR(tip_pose(0, 3), -L1, EPSILON); EXPECT_NEAR(tip_pose(1, 3), 0, EPSILON); EXPECT_NEAR(tip_pose(2, 3), L0 - L2 - L3, EPSILON); // wrong link name std::string link_name = "wrong_link_name"; - EXPECT_FALSE(pilz_industrial_motion_planner::computeLinkFK(planning_scene_, link_name, test_state, tip_pose)); + EXPECT_FALSE(pilz_industrial_motion_planner::computeLinkFK(*robot_state_, link_name, test_state, tip_pose)); } /** diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp index 2d043eb0fc..7e462e287a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp @@ -264,35 +264,12 @@ TEST_F(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping) EXPECT_EQ(nocm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } - { - auto cjmiss_ex = std::make_shared(""); - EXPECT_EQ(cjmiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); - } - { auto cifgi_ex = std::make_shared(""); EXPECT_EQ(cifgi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); } } -/** - * @brief test invalid motion plan request with incomplete start state and - * cartesian goal - */ -TEST_F(TrajectoryGeneratorCIRCTest, incompleteStartState) -{ - auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") }; - - planning_interface::MotionPlanRequest req{ circ.toRequest() }; - EXPECT_GT(req.start_state.joint_state.name.size(), 1u); - req.start_state.joint_state.name.resize(1); - req.start_state.joint_state.position.resize(1); // prevent failing check for equal sizes - - planning_interface::MotionPlanResponse res; - circ_->generate(planning_scene_, req, res); - EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); -} - /** * @brief test invalid motion plan request with non zero start velocity */ diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp index 7d2b375b41..cde5c06618 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp @@ -198,11 +198,6 @@ TEST_F(TrajectoryGeneratorLINTest, TestExceptionErrorCodeMapping) EXPECT_EQ(jnm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } - { - auto ljmiss_ex = std::make_shared(""); - EXPECT_EQ(ljmiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); - } - { auto lifgi_ex = std::make_shared(""); EXPECT_EQ(lifgi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION); @@ -415,24 +410,6 @@ TEST_F(TrajectoryGeneratorLINTest, IncorrectJointNumber) EXPECT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); } -/** - * @brief test invalid motion plan request with incomplete start state and - * cartesian goal - */ -TEST_F(TrajectoryGeneratorLINTest, cartGoalIncompleteStartState) -{ - // construct motion plan request - moveit_msgs::msg::MotionPlanRequest lin_cart_req{ tdp_->getLinCart("lin2").toRequest() }; - EXPECT_GT(lin_cart_req.start_state.joint_state.name.size(), 1u); - lin_cart_req.start_state.joint_state.name.resize(1); - lin_cart_req.start_state.joint_state.position.resize(1); // prevent failing check for equal sizes - - // generate lin trajectory - planning_interface::MotionPlanResponse res; - lin_->generate(planning_scene_, lin_cart_req, res); - EXPECT_EQ(res.error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE); -} - /** * @brief Set a frame id in goal constraint with cartesian goal on both position * and orientation constraints