From 2324407d5848ff4fa74a41708da04799c13ffc1d 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 (cherry picked from commit 70e1aae8bd2ee58baf136db74ca3b831186ec8fe) # Conflicts: # moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h # moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp # moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp # moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_circ.cpp # moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_generator_lin.cpp --- .../planning_context_base.h | 4 ++ .../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 | 46 +++++++------ .../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 | 8 +-- .../src/unittest_trajectory_generator_lin.cpp | 8 +-- 14 files changed, 101 insertions(+), 126 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 bef598c805..9208428a32 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 @@ -133,11 +133,15 @@ bool pilz_industrial_motion_planner::PlanningContextBase::solve(plan // res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN; // return false; // TODO } +<<<<<<< HEAD RCLCPP_ERROR(rclcpp::get_logger("moveit.pilz_industrial_motion_planner.planning_context_base"), "Using solve on a terminated planning context!"); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; return false; +======= + generator_.generate(getPlanningScene(), request_, res); +>>>>>>> 70e1aae8b (Ports moveit/moveit/pull/3519 to ros2 (#3055)) } template 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 7a81fa9eea..fc398e5f0b 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 a24a7395c6..fbde978692 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 @@ -113,12 +113,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; @@ -126,6 +127,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 }; /** @@ -201,7 +203,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 @@ -228,14 +230,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 b79eafa2b2..87d9c0ead9 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 f1a2dfcf7e..f586918959 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 0fa35a84ef..17e87b8f13 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 @@ -97,7 +97,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(LOGGER, "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 13fc0243e3..57a7b08ca0 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -59,12 +59,15 @@ bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::Plannin return false; } +<<<<<<< HEAD if (!robot_model->getJointModelGroup(group_name)->canSetStateFromIK(link_name)) { RCLCPP_ERROR_STREAM(LOGGER, "No valid IK solver exists for " << link_name << " in planning group " << group_name); return false; } +======= +>>>>>>> 70e1aae8b (Ports moveit/moveit/pull/3519 to ros2 (#3055)) if (frame_id != robot_model->getModelFrame()) { RCLCPP_ERROR_STREAM(LOGGER, "Given frame (" << frame_id << ") is unequal to model frame(" @@ -76,18 +79,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); } @@ -115,25 +122,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(LOGGER, "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; } @@ -569,17 +573,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 154eac5aa1..7e050ec180 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -114,11 +114,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"); @@ -151,19 +146,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 (auto const& 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)) { @@ -182,7 +169,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); @@ -208,7 +196,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 << '\"'; @@ -222,8 +211,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) { @@ -240,21 +229,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, @@ -310,7 +300,7 @@ bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& res.planner_id = req.planner_id; try { - validateRequest(req); + validateRequest(req, scene->getCurrentState()); } catch (const MoveItErrorCodeException& ex) { @@ -332,7 +322,7 @@ bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& return false; } - MotionPlanInfo plan_info; + MotionPlanInfo plan_info(scene, req); try { extractMotionPlanInfo(scene, req, plan_info); @@ -348,7 +338,7 @@ bool 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) { @@ -358,10 +348,36 @@ bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& return false; } +<<<<<<< HEAD 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); return true; +======= + 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]; + } + } +>>>>>>> 70e1aae8b (Ports moveit/moveit/pull/3519 to ros2 (#3055)) } } // 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 512497380f..5995cf496c 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 @@ -92,6 +92,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()) @@ -118,7 +119,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 @@ -138,22 +139,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 8dbed6dfa2..535fa5f299 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 @@ -69,6 +69,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()) @@ -91,9 +92,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 @@ -113,24 +112,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 bb665bca53..c1a0e62b8a 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 @@ -215,13 +215,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 aae4e64880..7b36e2dd06 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_; @@ -190,27 +192,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 df2f5ad69f..a07055825e 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,11 +264,6 @@ 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); @@ -276,6 +271,7 @@ TEST_F(TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping) } /** +<<<<<<< HEAD * @brief test invalid motion plan request with incomplete start state and * cartesian goal */ @@ -294,6 +290,8 @@ TEST_F(TrajectoryGeneratorCIRCTest, incompleteStartState) } /** +======= +>>>>>>> 70e1aae8b (Ports moveit/moveit/pull/3519 to ros2 (#3055)) * @brief test invalid motion plan request with non zero start velocity */ TEST_F(TrajectoryGeneratorCIRCTest, nonZeroStartVelocity) 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 4d093276e5..e23f074c12 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); @@ -414,6 +409,7 @@ TEST_F(TrajectoryGeneratorLINTest, IncorrectJointNumber) } /** +<<<<<<< HEAD * @brief test invalid motion plan request with incomplete start state and * cartesian goal */ @@ -432,6 +428,8 @@ TEST_F(TrajectoryGeneratorLINTest, cartGoalIncompleteStartState) } /** +======= +>>>>>>> 70e1aae8b (Ports moveit/moveit/pull/3519 to ros2 (#3055)) * @brief Set a frame id in goal constraint with cartesian goal on both position * and orientation constraints */