From d2ab1b6ba6fc5851311302c5ccfefad7f42ee6e5 Mon Sep 17 00:00:00 2001 From: Brendan Burns <5751682+brendandburns@users.noreply.github.com> Date: Sun, 27 Oct 2024 04:55:43 -0700 Subject: [PATCH 01/12] Switch to get for Dict lookup to prevent KeyError (#3043) --- moveit_configs_utils/moveit_configs_utils/launches.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_configs_utils/moveit_configs_utils/launches.py b/moveit_configs_utils/moveit_configs_utils/launches.py index a423395000..142193de44 100644 --- a/moveit_configs_utils/moveit_configs_utils/launches.py +++ b/moveit_configs_utils/moveit_configs_utils/launches.py @@ -249,7 +249,7 @@ def generate_move_group_launch(moveit_config): parameters=move_group_params, extra_debug_args=["--debug"], # Set the display variable, in case OpenGL code is used internally - additional_env={"DISPLAY": os.environ["DISPLAY"]}, + additional_env={"DISPLAY": os.environ.get("DISPLAY", "")}, ) return ld From 02ebcba723d80392012bbfa7f495860be464041b Mon Sep 17 00:00:00 2001 From: Tom Noble <53005340+TSNoble@users.noreply.github.com> Date: Mon, 28 Oct 2024 20:37:34 +0000 Subject: [PATCH 02/12] Enhancement/moveit ros1 ports (#3041) * Ports https://github.com/moveit/moveit/pull/3592/ * Ports https://github.com/moveit/moveit/pull/3590 * Fixes compile errors --------- Co-authored-by: Sebastian Jahr --- .../src/command_list_manager.cpp | 3 +- .../src/move_group_interface.cpp | 57 +++++++------------ 2 files changed, 22 insertions(+), 38 deletions(-) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp index 49e58df4bd..36458d5423 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp @@ -182,7 +182,8 @@ void CommandListManager::setStartState(const MotionResponseCont& motion_plan_res RobotState_OptRef rob_state_op{ getPreviousEndState(motion_plan_responses, group_name) }; if (rob_state_op) { - moveit::core::robotStateToRobotStateMsg(rob_state_op.value(), start_state); + moveit::core::robotStateToRobotStateMsg(rob_state_op.value(), start_state, false); + start_state.is_diff = true; } } diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 9281ac82d3..e729707913 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -135,6 +135,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl throw std::runtime_error(error); } + setStartStateToCurrentState(); joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name); joint_state_target_ = std::make_shared(getRobotModel()); @@ -406,28 +407,30 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return *joint_state_target_; } + void setStartState(const moveit_msgs::msg::RobotState& start_state) + { + considered_start_state_ = start_state; + } + void setStartState(const moveit::core::RobotState& start_state) { - considered_start_state_ = std::make_shared(start_state); + considered_start_state_ = moveit_msgs::msg::RobotState(); + moveit::core::robotStateToRobotStateMsg(start_state, considered_start_state_, true); } void setStartStateToCurrentState() { - considered_start_state_.reset(); + // set message to empty diff + considered_start_state_ = moveit_msgs::msg::RobotState(); + considered_start_state_.is_diff = true; } moveit::core::RobotStatePtr getStartState() { - if (considered_start_state_) - { - return considered_start_state_; - } - else - { - moveit::core::RobotStatePtr s; - getCurrentState(s); - return s; - } + moveit::core::RobotStatePtr s; + getCurrentState(s); + moveit::core::robotStateMsgToRobotState(considered_start_state_, *s, true); + return s; } bool setJointValueTarget(const geometry_msgs::msg::Pose& eef_pose, const std::string& end_effector_link, @@ -862,8 +865,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl auto req = std::make_shared(); moveit_msgs::srv::GetCartesianPath::Response::SharedPtr response; - constructRobotState(req->start_state); - + req->start_state = considered_start_state_; req->group_name = opt_.group_name; req->header.frame_id = getPoseReferenceFrame(); req->header.stamp = getClock()->now(); @@ -1008,14 +1010,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl void constructRobotState(moveit_msgs::msg::RobotState& state) const { - if (considered_start_state_) - { - moveit::core::robotStateToRobotStateMsg(*considered_start_state_, state); - } - else - { - state.is_diff = true; - } + state = considered_start_state_; } void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request) const @@ -1028,8 +1023,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl request.pipeline_id = planning_pipeline_id_; request.planner_id = planner_id_; request.workspace_parameters = workspace_parameters_; - - constructRobotState(request.start_state); + request.start_state = considered_start_state_; if (active_target_ == JOINT) { @@ -1217,7 +1211,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl std::shared_ptr> execute_action_client_; // general planning params - moveit::core::RobotStatePtr considered_start_state_; + moveit_msgs::msg::RobotState considered_start_state_; moveit_msgs::msg::WorkspaceParameters workspace_parameters_; double allowed_planning_time_; std::string planning_pipeline_id_; @@ -1484,18 +1478,7 @@ void MoveGroupInterface::stop() void MoveGroupInterface::setStartState(const moveit_msgs::msg::RobotState& start_state) { - moveit::core::RobotStatePtr rs; - if (start_state.is_diff) - { - impl_->getCurrentState(rs); - } - else - { - rs = std::make_shared(getRobotModel()); - rs->setToDefaultValues(); // initialize robot state values for conversion - } - moveit::core::robotStateMsgToRobotState(start_state, *rs); - setStartState(*rs); + impl_->setStartState(start_state); } void MoveGroupInterface::setStartState(const moveit::core::RobotState& start_state) From ae05a5e89ccef1e58a0cbe1a6d238f095bfc500c Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 29 Oct 2024 10:15:07 -0600 Subject: [PATCH 03/12] set filter state when no commands (#3027) Signed-off-by: Paul Gesel --- moveit_ros/moveit_servo/src/servo_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index bfc989adf3..9eae434869 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -383,6 +383,7 @@ void ServoNode::servoLoop() { // if no new command was created, use current robot state updateSlidingWindow(current_state, joint_cmd_rolling_window_, servo_params_.max_expected_latency, cur_time); + servo_->resetSmoothing(current_state); } status_msg.code = static_cast(servo_->getStatus()); From 08da0b607c2c168604a7b8b6868e52844a47789d Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Wed, 30 Oct 2024 10:48:12 -0400 Subject: [PATCH 04/12] Fix flipped comments in `joint_model.h` (#3047) --- .../robot_model/include/moveit/robot_model/joint_model.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model.h index 019e108ec6..d75796440f 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model.h +++ b/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 */ From d5f763ed18b4a0cedcb2efc40961885f30387146 Mon Sep 17 00:00:00 2001 From: Dongya Jiang <1983534+patrickKXMD@users.noreply.github.com> Date: Thu, 31 Oct 2024 15:41:41 +0800 Subject: [PATCH 05/12] [moveit_servo] fix: ensure ee_pose on planning_frame (#3046) Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- moveit_ros/moveit_servo/src/utils/command.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index c9337deb88..0470edebe8 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -245,7 +245,8 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co Eigen::Vector cartesian_position_delta; // Compute linear and angular change needed. - const Eigen::Isometry3d ee_pose{ robot_state->getGlobalLinkTransform(ee_frame) }; + const Eigen::Isometry3d ee_pose{ robot_state->getGlobalLinkTransform(planning_frame).inverse() * + robot_state->getGlobalLinkTransform(ee_frame) }; const Eigen::Quaterniond q_current(ee_pose.rotation()); Eigen::Quaterniond q_target(command.pose.rotation()); Eigen::Vector3d translation_error = command.pose.translation() - ee_pose.translation(); From b3b1f73b3295abc14f1cd7dbbe851fa594b3b58b Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Mon, 4 Nov 2024 04:18:41 -0500 Subject: [PATCH 06/12] Wait for nonzero joint states in PSM in Servo CPP integration test (#3056) * Wait for nonzero joint states in PSM in Servo CPP integration test * Fix fail wording * Revert locked planning scene change * Lock down that planning scene --- .../moveit_servo/src/collision_monitor.cpp | 8 ++--- .../moveit_servo/tests/servo_cpp_fixture.hpp | 30 ++++++++++++++++++- .../moveit_servo/tests/test_integration.cpp | 16 +++++++--- 3 files changed, 45 insertions(+), 9 deletions(-) diff --git a/moveit_ros/moveit_servo/src/collision_monitor.cpp b/moveit_ros/moveit_servo/src/collision_monitor.cpp index c3a886cff8..f1e8c8c911 100644 --- a/moveit_ros/moveit_servo/src/collision_monitor.cpp +++ b/moveit_ros/moveit_servo/src/collision_monitor.cpp @@ -105,15 +105,15 @@ void CollisionMonitor::checkCollisions() if (servo_params_.check_collisions) { + // Get a read-only copy of the planning scene. + planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_); + // Fetch latest robot state using planning scene instead of state monitor due to // https://github.com/moveit/moveit2/issues/2748 - robot_state_ = planning_scene_monitor_->getPlanningScene()->getCurrentState(); + robot_state_ = locked_scene->getCurrentState(); // This must be called before doing collision checking. robot_state_.updateCollisionBodyTransforms(); - // Get a read-only copy of planning scene. - planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_); - // Check collision with environment. scene_collision_result_.clear(); locked_scene->getCollisionEnv()->checkRobotCollision(scene_collision_request_, scene_collision_result_, diff --git a/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp index 7e98f8e680..86e9c657ff 100644 --- a/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp +++ b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp @@ -62,6 +62,24 @@ class ServoCppFixture : public testing::Test planning_scene_monitor_ = moveit_servo::createPlanningSceneMonitor(servo_test_node_, servo_params_); + // Wait until the joint configuration is nonzero before starting MoveIt Servo. + int num_tries = 0; + const int max_tries = 20; + while (true) + { + const auto q = getCurrentJointPositions("panda_arm"); + if (q.norm() > 0.0) + { + break; + } + if (num_tries > max_tries) + { + FAIL() << "Robot joint configuration did not reach expected state after some time. Test is flaky."; + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + num_tries++; + } + servo_test_instance_ = std::make_shared(servo_test_node_, servo_param_listener_, planning_scene_monitor_); } @@ -69,7 +87,17 @@ class ServoCppFixture : public testing::Test /// Helper function to get the current pose of a specified frame. Eigen::Isometry3d getCurrentPose(const std::string& target_frame) const { - return planning_scene_monitor_->getStateMonitor()->getCurrentState()->getGlobalLinkTransform(target_frame); + planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_); + return locked_scene->getCurrentState().getGlobalLinkTransform(target_frame); + } + + /// Helper function to get the joint configuration of a group. + Eigen::VectorXd getCurrentJointPositions(const std::string& group_name) const + { + planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_); + std::vector joint_positions; + locked_scene->getCurrentState().copyJointGroupPositions(group_name, joint_positions); + return Eigen::Map(joint_positions.data(), joint_positions.size()); } std::shared_ptr servo_test_node_; diff --git a/moveit_ros/moveit_servo/tests/test_integration.cpp b/moveit_ros/moveit_servo/tests/test_integration.cpp index d04587a9ab..645b2835cf 100644 --- a/moveit_ros/moveit_servo/tests/test_integration.cpp +++ b/moveit_ros/moveit_servo/tests/test_integration.cpp @@ -46,10 +46,12 @@ namespace TEST_F(ServoCppFixture, JointJogTest) { + planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_); + auto robot_state = std::make_shared(locked_scene->getCurrentState()); + moveit_servo::StatusCode status_curr, status_next, status_initial; moveit_servo::JointJogCommand joint_jog_z{ { "panda_joint7" }, { 1.0 } }; moveit_servo::JointJogCommand zero_joint_jog; - moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); // Compute next state. servo_test_instance_->setCommandType(moveit_servo::CommandType::JOINT_JOG); @@ -72,10 +74,12 @@ TEST_F(ServoCppFixture, JointJogTest) TEST_F(ServoCppFixture, TwistTest) { + planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_); + auto robot_state = std::make_shared(locked_scene->getCurrentState()); + moveit_servo::StatusCode status_curr, status_next, status_initial; moveit_servo::TwistCommand twist_non_zero{ "panda_link0", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.1 } }; moveit_servo::TwistCommand twist_zero{ "panda_link0", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; - moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); servo_test_instance_->setCommandType(moveit_servo::CommandType::TWIST); status_initial = servo_test_instance_->getStatus(); @@ -97,10 +101,12 @@ TEST_F(ServoCppFixture, TwistTest) TEST_F(ServoCppFixture, NonPlanningFrameTwistTest) { + planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_); + auto robot_state = std::make_shared(locked_scene->getCurrentState()); + moveit_servo::StatusCode status_curr, status_next, status_initial; moveit_servo::TwistCommand twist_non_zero{ "panda_link8", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.1 } }; moveit_servo::TwistCommand twist_zero{ "panda_link8", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; - moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); servo_test_instance_->setCommandType(moveit_servo::CommandType::TWIST); status_initial = servo_test_instance_->getStatus(); @@ -122,6 +128,9 @@ TEST_F(ServoCppFixture, NonPlanningFrameTwistTest) TEST_F(ServoCppFixture, PoseTest) { + planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_); + auto robot_state = std::make_shared(locked_scene->getCurrentState()); + moveit_servo::StatusCode status_curr, status_next, status_initial; moveit_servo::PoseCommand zero_pose, non_zero_pose; zero_pose.frame_id = "panda_link0"; @@ -135,7 +144,6 @@ TEST_F(ServoCppFixture, PoseTest) status_initial = servo_test_instance_->getStatus(); ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING); - moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState(); moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(robot_state, zero_pose); status_curr = servo_test_instance_->getStatus(); ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING); From 70e1aae8bd2ee58baf136db74ca3b831186ec8fe Mon Sep 17 00:00:00 2001 From: Tom Noble Date: Tue, 5 Nov 2024 11:14:25 +0000 Subject: [PATCH 07/12] 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 From 41613cb9d787b8eed949b3919aeb7b37f7422693 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 5 Nov 2024 19:20:30 +0100 Subject: [PATCH 08/12] CI: Allow installation of ros-*-rmw-connextdds in docker images (#3063) --- .docker/release/Dockerfile | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.docker/release/Dockerfile b/.docker/release/Dockerfile index 0c02814cbe..7e819456b4 100644 --- a/.docker/release/Dockerfile +++ b/.docker/release/Dockerfile @@ -5,6 +5,9 @@ ARG ROS_DISTRO=rolling FROM ros:${ROS_DISTRO}-ros-base LABEL maintainer Robert Haschke rhaschke@techfak.uni-bielefeld.de +# 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 && \ From 36487d1b5bece864da0065c1159f5f8f3ff5c9b5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 5 Nov 2024 20:32:21 +0100 Subject: [PATCH 09/12] Cleanup #3056 (#3058) * Cleanup #3056 * Switch to PSM's waitForCurrentRobotState() * Back to StateMonitor usage and explicitly propagate state update to PlanningScene --- .../moveit_servo/tests/servo_cpp_fixture.hpp | 30 ++++--------------- 1 file changed, 5 insertions(+), 25 deletions(-) diff --git a/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp index 86e9c657ff..b8a318a9b6 100644 --- a/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp +++ b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp @@ -61,24 +61,13 @@ class ServoCppFixture : public testing::Test servo_params_ = servo_param_listener_->get_params(); planning_scene_monitor_ = moveit_servo::createPlanningSceneMonitor(servo_test_node_, servo_params_); - - // Wait until the joint configuration is nonzero before starting MoveIt Servo. - int num_tries = 0; - const int max_tries = 20; - while (true) + // Wait for complete state update before starting MoveIt Servo. + if (!planning_scene_monitor_->getStateMonitor()->waitForCompleteState("panda_arm", 1.0)) { - const auto q = getCurrentJointPositions("panda_arm"); - if (q.norm() > 0.0) - { - break; - } - if (num_tries > max_tries) - { - FAIL() << "Robot joint configuration did not reach expected state after some time. Test is flaky."; - } - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - num_tries++; + FAIL() << "Could not retrieve complete robot state"; } + // Forward state update to planning scene + planning_scene_monitor_->updateSceneWithCurrentState(); servo_test_instance_ = std::make_shared(servo_test_node_, servo_param_listener_, planning_scene_monitor_); @@ -91,15 +80,6 @@ class ServoCppFixture : public testing::Test return locked_scene->getCurrentState().getGlobalLinkTransform(target_frame); } - /// Helper function to get the joint configuration of a group. - Eigen::VectorXd getCurrentJointPositions(const std::string& group_name) const - { - planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_); - std::vector joint_positions; - locked_scene->getCurrentState().copyJointGroupPositions(group_name, joint_positions); - return Eigen::Map(joint_positions.data(), joint_positions.size()); - } - std::shared_ptr servo_test_node_; std::shared_ptr servo_param_listener_; servo::Params servo_params_; From fe3a0907779b1a105d899a78e5399728bbd58850 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 6 Nov 2024 02:37:23 -0700 Subject: [PATCH 10/12] Fix createTrajectoryMessage (#3064) * Fix createTrajectoryMessage Signed-off-by: Paul Gesel * Use std::ceil to handle non-aligned sampling_rate Signed-off-by: Paul Gesel --------- Signed-off-by: Paul Gesel --- moveit_core/trajectory_processing/src/trajectory_tools.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/trajectory_processing/src/trajectory_tools.cpp b/moveit_core/trajectory_processing/src/trajectory_tools.cpp index b15f0ca9a3..cca3586187 100644 --- a/moveit_core/trajectory_processing/src/trajectory_tools.cpp +++ b/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(sampling_rate); - const int n_samples = static_cast(trajectory.getDuration() / time_step) + 1; + const int n_samples = static_cast(std::ceil(trajectory.getDuration() / time_step)) + 1; trajectory_msg.points.reserve(n_samples); for (int sample = 0; sample < n_samples; ++sample) { From dfb3877132ee7c1d2dc379c3b4665912b37001c1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 6 Nov 2024 13:55:44 +0100 Subject: [PATCH 11/12] Port fixes for handling orientation constraints (#3052) * PoseModelStateSpace: Reintroduce Cartesian interpolation The problem with potential jumps due to Cartesian interpolation in principle remains, which is a dilemma of the current design: - We do need Cartesian interpolation to reduce the risk of state rejection due to a failing path constraint, which currently cannot be checked during interpolation. - Ideally, we would do joint interpolation and only resort to Cartesian interpolation if the path constraint is violated, i.e. follow a similar approach as in #3618. However, this would require access to the path constraint... * Fix orientation constraints The tolerance of orientation constraints needs to be interpreted w.r.t. the given frame_id of the constraint instead of the target frame. Not doing so, a large tolerance is interpreted about the wrong axis and resolving constraint frames fails. Unfortunately, the proposed approach only works for the ROTATION_VECTOR representation. * Address review --- .../src/default_constraint_samplers.cpp | 13 +++-- .../kinematic_constraint.h | 49 +++++++++++++------ .../src/kinematic_constraint.cpp | 10 ++-- .../kinematic_constraints/src/utils.cpp | 9 ++++ .../test/test_orientation_constraints.cpp | 36 ++++++++++++++ .../work_space/pose_model_state_space.h | 1 + .../work_space/pose_model_state_space.cpp | 28 ++++++++++- 7 files changed, 117 insertions(+), 29 deletions(-) diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index f5db2a6968..a1869ccc3d 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -515,6 +515,9 @@ 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 @@ -522,14 +525,10 @@ bool IKConstraintSampler::samplePose(Eigen::Vector3d& pos, Eigen::Quaterniond& q /* 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 diff --git a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h b/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h index 0d5d2e9674..5684d55fcf 100644 --- a/moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h +++ b/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 diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 33e4efbbde..a56678402e 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/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 { diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index 6a66f6826f..96fa5a70bc 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/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); diff --git a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp b/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp index ea0e49608d..1a26e34abe 100644 --- a/moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp +++ b/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); diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h index 6f7837ae3a..4ebe272e8d 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h +++ b/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 poses_; + double jump_factor_; }; } // namespace ompl_interface diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp index 12d95371ab..600e5281de 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp +++ b/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()->poses[i], to->as()->poses[i], t, + state->as()->poses[i]); + } + + // the call above may reset all flags for state; but we know the pose we want flag should be set + state->as()->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()->markInvalid(); + } } void PoseModelStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ) From 1a77d7d5154108b6ad0681f17502092a0b374a5e Mon Sep 17 00:00:00 2001 From: Dongya Jiang <1983534+patrickKXMD@users.noreply.github.com> Date: Thu, 7 Nov 2024 17:27:43 +0800 Subject: [PATCH 12/12] [moveit_servo] avoid race condition when calling ~/pause_servo (#3059) Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp | 3 +++ moveit_ros/moveit_servo/src/servo_node.cpp | 2 ++ 2 files changed, 5 insertions(+) diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp index 56ba8f595b..b825c0664b 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.hpp @@ -133,6 +133,9 @@ class ServoNode // Threads used by ServoNode std::thread servo_loop_thread_; + // Locks for threads safety + std::mutex lock_; + // rolling window of joint commands std::deque joint_cmd_rolling_window_; }; diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 9eae434869..0226472215 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -151,6 +151,7 @@ void ServoNode::pauseServo(const std::shared_ptr lock_guard(lock_); // Reset the smoothing plugin with the robot's current state in case the robot moved between pausing and unpausing. last_commanded_state_ = servo_->getCurrentRobotState(true /* block for current robot state */); servo_->resetSmoothing(last_commanded_state_); @@ -320,6 +321,7 @@ void ServoNode::servoLoop() continue; } + std::lock_guard lock_guard(lock_); const bool use_trajectory = servo_params_.command_out_type == "trajectory_msgs/JointTrajectory"; const auto cur_time = node_->now();