From c931acfa6408157d3c902ee942fa9f6d3926bd79 Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Wed, 21 Feb 2024 12:58:51 -0500 Subject: [PATCH 1/8] Validate connect stage deviation: end trajectory with goal If not within the threshold, mark solution as a failure and set comment. --- core/src/stages/connect.cpp | 45 +++++++++++++++++++++++++++++++++++++ 1 file changed, 45 insertions(+) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 5f71defb9..a224b000d 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -43,6 +43,8 @@ #include #include +#include + using namespace trajectory_processing; namespace moveit { @@ -146,6 +148,8 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { intermediate_scenes.push_back(start); bool success = false; + bool deviation = false; + std::stringstream deviation_comment; std::vector positions; for (const GroupPlannerVector::value_type& pair : planner_) { // set intermediate goal state @@ -164,6 +168,45 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { if (!success) break; + // validate position deviation from trajectory last waypoint to goal + const std::size_t waypoint_count = trajectory->getWayPointCount(); + + if (waypoint_count) { + for (const moveit::core::JointModel* jm : jmg->getJointModels()) { + const unsigned int num = jm->getVariableCount(); + + if (!num) + continue; + + Eigen::Map positions_goal(goal_state.getJointPositions(jm), num); + Eigen::Map positions_last_waypoint( + trajectory->getWayPoint(waypoint_count - 1).getJointPositions(jm), num); + + for (std::size_t i = 0; i < num; ++i) { + double min_distance = angles::shortest_angular_distance(positions_last_waypoint[i], positions_goal[i]); + ROS_DEBUG_STREAM_NAMED( + "Connect", "angular deviation: " << min_distance << " between trajectory last waypoint and goal."); + + if (std::abs(min_distance) > 1e-4) { + deviation_comment << "Deviation in joint " << jm->getName() << ": [" + << positions_last_waypoint.transpose() << "] != [" << positions_goal.transpose() + << "]"; + ROS_ERROR_STREAM_NAMED("Connect", deviation_comment.str()); + + deviation = true; + success = false; + break; + } + } + + if (deviation) + break; + } + } + + if (deviation) + break; + // continue from reached state start = end; } @@ -175,6 +218,8 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { solution = makeSequential(sub_trajectories, intermediate_scenes, from, to); if (!success) // error during sequential planning solution->markAsFailure(); + if (deviation) // deviation error + solution->setComment(deviation_comment.str()); connect(from, to, solution); } From 7c317b93fc89a943b2a81238098148fe967556fb Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Thu, 22 Feb 2024 09:35:23 -0500 Subject: [PATCH 2/8] [fix] compute shortest angular distance only on revolute joints --- core/src/stages/connect.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index a224b000d..00419cc24 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -182,8 +182,13 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { Eigen::Map positions_last_waypoint( trajectory->getWayPoint(waypoint_count - 1).getJointPositions(jm), num); + double min_distance; for (std::size_t i = 0; i < num; ++i) { - double min_distance = angles::shortest_angular_distance(positions_last_waypoint[i], positions_goal[i]); + if (jm->getType() == robot_state::JointModel::REVOLUTE) + min_distance = angles::shortest_angular_distance(positions_last_waypoint[i], positions_goal[i]); + else + min_distance = positions_last_waypoint[i], positions_goal[i]; + ROS_DEBUG_STREAM_NAMED( "Connect", "angular deviation: " << min_distance << " between trajectory last waypoint and goal."); From b42c0b1f597cda7ef9fbb5c3a9422e0bdda11277 Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Thu, 22 Feb 2024 10:14:53 -0500 Subject: [PATCH 3/8] [refac] move validation into it's own method --- .../moveit/task_constructor/stages/connect.h | 3 + core/src/stages/connect.cpp | 96 ++++++++++--------- 2 files changed, 54 insertions(+), 45 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index bb64dbb74..b7af957fc 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -65,6 +65,9 @@ class Connect : public Connecting { protected: bool compatible(const InterfaceState& from_state, const InterfaceState& to_state) const override; + bool validateEndTrajectoryDeviation(const moveit::core::JointModelGroup* jmg, + const robot_trajectory::RobotTrajectoryPtr trajectory, + const moveit::core::RobotState& goal_state, std::string& comment); public: enum MergeMode diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 00419cc24..f601b7f1a 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -134,6 +134,51 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState& return true; } +bool Connect::validateEndTrajectoryDeviation(const moveit::core::JointModelGroup* jmg, + const robot_trajectory::RobotTrajectoryPtr trajectory, + const moveit::core::RobotState& goal_state, std::string& comment) { + const std::size_t waypoint_count = trajectory->getWayPointCount(); + + if (!waypoint_count) { + comment = "Cannot validate end trajectory deviation with an empty trajectory"; + return false; + } + + for (const moveit::core::JointModel* jm : jmg->getJointModels()) { + const unsigned int num = jm->getVariableCount(); + + if (!num) + continue; + + Eigen::Map positions_goal(goal_state.getJointPositions(jm), num); + Eigen::Map positions_last_waypoint( + trajectory->getWayPoint(waypoint_count - 1).getJointPositions(jm), num); + + double min_distance; + for (std::size_t i = 0; i < num; ++i) { + if (jm->getType() == robot_state::JointModel::REVOLUTE) + min_distance = angles::shortest_angular_distance(positions_last_waypoint[i], positions_goal[i]); + else + min_distance = positions_last_waypoint[i], positions_goal[i]; + + ROS_DEBUG_STREAM_NAMED("Connect", + "angular deviation: " << min_distance << " between trajectory last waypoint and goal."); + + if (std::abs(min_distance) > 1e-4) { + std::stringstream ss; + ss << "Deviation in joint " << jm->getName() << ": [" << positions_last_waypoint.transpose() << "] != [" + << positions_goal.transpose() << "]"; + comment = ss.str(); + ROS_ERROR_STREAM_NAMED("Connect", comment); + + return false; + } + } + } + + return true; +} + void Connect::compute(const InterfaceState& from, const InterfaceState& to) { const auto& props = properties(); double timeout = this->timeout(); @@ -148,8 +193,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { intermediate_scenes.push_back(start); bool success = false; - bool deviation = false; - std::stringstream deviation_comment; + std::string deviation_comment; std::vector positions; for (const GroupPlannerVector::value_type& pair : planner_) { // set intermediate goal state @@ -168,48 +212,10 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { if (!success) break; - // validate position deviation from trajectory last waypoint to goal - const std::size_t waypoint_count = trajectory->getWayPointCount(); - - if (waypoint_count) { - for (const moveit::core::JointModel* jm : jmg->getJointModels()) { - const unsigned int num = jm->getVariableCount(); - - if (!num) - continue; - - Eigen::Map positions_goal(goal_state.getJointPositions(jm), num); - Eigen::Map positions_last_waypoint( - trajectory->getWayPoint(waypoint_count - 1).getJointPositions(jm), num); - - double min_distance; - for (std::size_t i = 0; i < num; ++i) { - if (jm->getType() == robot_state::JointModel::REVOLUTE) - min_distance = angles::shortest_angular_distance(positions_last_waypoint[i], positions_goal[i]); - else - min_distance = positions_last_waypoint[i], positions_goal[i]; + // validate position deviation: trajectory last waypoint to goal + success = validateEndTrajectoryDeviation(jmg, trajectory, goal_state, deviation_comment); - ROS_DEBUG_STREAM_NAMED( - "Connect", "angular deviation: " << min_distance << " between trajectory last waypoint and goal."); - - if (std::abs(min_distance) > 1e-4) { - deviation_comment << "Deviation in joint " << jm->getName() << ": [" - << positions_last_waypoint.transpose() << "] != [" << positions_goal.transpose() - << "]"; - ROS_ERROR_STREAM_NAMED("Connect", deviation_comment.str()); - - deviation = true; - success = false; - break; - } - } - - if (deviation) - break; - } - } - - if (deviation) + if (!success) break; // continue from reached state @@ -223,8 +229,8 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { solution = makeSequential(sub_trajectories, intermediate_scenes, from, to); if (!success) // error during sequential planning solution->markAsFailure(); - if (deviation) // deviation error - solution->setComment(deviation_comment.str()); + if (!deviation_comment.empty()) // deviation error + solution->setComment(deviation_comment); connect(from, to, solution); } From 26e851926306302d45c5ff8f4ce4fe81d990719b Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Fri, 23 Feb 2024 08:48:14 -0500 Subject: [PATCH 4/8] [fix] compute joint difference --- core/src/stages/connect.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index f601b7f1a..6884eea96 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -159,7 +159,7 @@ bool Connect::validateEndTrajectoryDeviation(const moveit::core::JointModelGroup if (jm->getType() == robot_state::JointModel::REVOLUTE) min_distance = angles::shortest_angular_distance(positions_last_waypoint[i], positions_goal[i]); else - min_distance = positions_last_waypoint[i], positions_goal[i]; + min_distance = positions_last_waypoint[i] - positions_goal[i]; ROS_DEBUG_STREAM_NAMED("Connect", "angular deviation: " << min_distance << " between trajectory last waypoint and goal."); From 0d50ec9a899ce295b12050fb05a5ad71d3efd508 Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Fri, 23 Feb 2024 10:45:23 -0500 Subject: [PATCH 5/8] [impl] expose deviation threshold to stage property --- core/include/moveit/task_constructor/stages/connect.h | 4 +++- core/src/stages/connect.cpp | 10 +++++++--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index b7af957fc..9fce57445 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -67,7 +67,8 @@ class Connect : public Connecting bool compatible(const InterfaceState& from_state, const InterfaceState& to_state) const override; bool validateEndTrajectoryDeviation(const moveit::core::JointModelGroup* jmg, const robot_trajectory::RobotTrajectoryPtr trajectory, - const moveit::core::RobotState& goal_state, std::string& comment); + const moveit::core::RobotState& goal_state, double max_joint_deviation, + std::string& comment); public: enum MergeMode @@ -79,6 +80,7 @@ class Connect : public Connecting using GroupPlannerVector = std::vector >; Connect(const std::string& name = "connect", const GroupPlannerVector& planners = {}); + void setMaxJointDeviation(double max_joint_deviation) { setProperty("max_joint_deviation", max_joint_deviation); } void setPathConstraints(moveit_msgs::Constraints path_constraints) { setProperty("path_constraints", std::move(path_constraints)); } diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 6884eea96..160d842b7 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -57,6 +57,8 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : auto& p = properties(); p.declare("merge_mode", WAYPOINTS, "merge mode"); + p.declare("max_joint_deviation", 1e-4, + "maximum allowed joint position difference between end-state and goal-sate "); p.declare("path_constraints", moveit_msgs::Constraints(), "constraints to maintain during trajectory"); properties().declare("merge_time_parameterization", @@ -136,7 +138,8 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState& bool Connect::validateEndTrajectoryDeviation(const moveit::core::JointModelGroup* jmg, const robot_trajectory::RobotTrajectoryPtr trajectory, - const moveit::core::RobotState& goal_state, std::string& comment) { + const moveit::core::RobotState& goal_state, double max_joint_deviation, + std::string& comment) { const std::size_t waypoint_count = trajectory->getWayPointCount(); if (!waypoint_count) { @@ -164,7 +167,7 @@ bool Connect::validateEndTrajectoryDeviation(const moveit::core::JointModelGroup ROS_DEBUG_STREAM_NAMED("Connect", "angular deviation: " << min_distance << " between trajectory last waypoint and goal."); - if (std::abs(min_distance) > 1e-4) { + if (std::abs(min_distance) > max_joint_deviation) { std::stringstream ss; ss << "Deviation in joint " << jm->getName() << ": [" << positions_last_waypoint.transpose() << "] != [" << positions_goal.transpose() << "]"; @@ -183,6 +186,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { const auto& props = properties(); double timeout = this->timeout(); MergeMode mode = props.get("merge_mode"); + double max_joint_deviation = props.get("max_joint_deviation"); const auto& path_constraints = props.get("path_constraints"); const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState(); @@ -213,7 +217,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { break; // validate position deviation: trajectory last waypoint to goal - success = validateEndTrajectoryDeviation(jmg, trajectory, goal_state, deviation_comment); + success = validateEndTrajectoryDeviation(jmg, trajectory, goal_state, max_joint_deviation, deviation_comment); if (!success) break; From fc6eacc4583051be431a8e8911a9005d1b633ca4 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 24 Feb 2024 22:43:36 +0100 Subject: [PATCH 6/8] simplify --- core/src/stages/connect.cpp | 67 +++++-------------------------------- 1 file changed, 8 insertions(+), 59 deletions(-) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 160d842b7..0ab68c23d 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -43,8 +43,6 @@ #include #include -#include - using namespace trajectory_processing; namespace moveit { @@ -57,8 +55,7 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : auto& p = properties(); p.declare("merge_mode", WAYPOINTS, "merge mode"); - p.declare("max_joint_deviation", 1e-4, - "maximum allowed joint position difference between end-state and goal-sate "); + p.declare("max_distance", 1e-4, "maximally accepted distance between end and goal sate"); p.declare("path_constraints", moveit_msgs::Constraints(), "constraints to maintain during trajectory"); properties().declare("merge_time_parameterization", @@ -136,57 +133,11 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState& return true; } -bool Connect::validateEndTrajectoryDeviation(const moveit::core::JointModelGroup* jmg, - const robot_trajectory::RobotTrajectoryPtr trajectory, - const moveit::core::RobotState& goal_state, double max_joint_deviation, - std::string& comment) { - const std::size_t waypoint_count = trajectory->getWayPointCount(); - - if (!waypoint_count) { - comment = "Cannot validate end trajectory deviation with an empty trajectory"; - return false; - } - - for (const moveit::core::JointModel* jm : jmg->getJointModels()) { - const unsigned int num = jm->getVariableCount(); - - if (!num) - continue; - - Eigen::Map positions_goal(goal_state.getJointPositions(jm), num); - Eigen::Map positions_last_waypoint( - trajectory->getWayPoint(waypoint_count - 1).getJointPositions(jm), num); - - double min_distance; - for (std::size_t i = 0; i < num; ++i) { - if (jm->getType() == robot_state::JointModel::REVOLUTE) - min_distance = angles::shortest_angular_distance(positions_last_waypoint[i], positions_goal[i]); - else - min_distance = positions_last_waypoint[i] - positions_goal[i]; - - ROS_DEBUG_STREAM_NAMED("Connect", - "angular deviation: " << min_distance << " between trajectory last waypoint and goal."); - - if (std::abs(min_distance) > max_joint_deviation) { - std::stringstream ss; - ss << "Deviation in joint " << jm->getName() << ": [" << positions_last_waypoint.transpose() << "] != [" - << positions_goal.transpose() << "]"; - comment = ss.str(); - ROS_ERROR_STREAM_NAMED("Connect", comment); - - return false; - } - } - } - - return true; -} - void Connect::compute(const InterfaceState& from, const InterfaceState& to) { const auto& props = properties(); double timeout = this->timeout(); MergeMode mode = props.get("merge_mode"); - double max_joint_deviation = props.get("max_joint_deviation"); + double max_distance = props.get("max_distance"); const auto& path_constraints = props.get("path_constraints"); const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState(); @@ -197,7 +148,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { intermediate_scenes.push_back(start); bool success = false; - std::string deviation_comment; + std::string comment; std::vector positions; for (const GroupPlannerVector::value_type& pair : planner_) { // set intermediate goal state @@ -216,11 +167,11 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { if (!success) break; - // validate position deviation: trajectory last waypoint to goal - success = validateEndTrajectoryDeviation(jmg, trajectory, goal_state, max_joint_deviation, deviation_comment); - - if (!success) + if (trajectory->getLastWayPoint().distance(goal_state, jmg) > max_distance) { + success = false; + comment = "Trajectory end-point deviates too much from goal state"; break; + } // continue from reached state start = end; @@ -232,9 +183,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) { if (!solution) // success == false or merging failed: store sequentially solution = makeSequential(sub_trajectories, intermediate_scenes, from, to); if (!success) // error during sequential planning - solution->markAsFailure(); - if (!deviation_comment.empty()) // deviation error - solution->setComment(deviation_comment); + solution->markAsFailure(comment); connect(from, to, solution); } From 7edc2f9d1f424c18830093596e6a3b6b90db53cf Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 25 Feb 2024 13:46:01 +0100 Subject: [PATCH 7/8] Add corresponding unit test --- core/test/test_move_to.cpp | 30 +++++++++++++++++++++++++++++- 1 file changed, 29 insertions(+), 1 deletion(-) diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp index 8e7ac023f..aaaf313eb 100644 --- a/core/test/test_move_to.cpp +++ b/core/test/test_move_to.cpp @@ -3,8 +3,10 @@ #include #include +#include #include #include +#include #include @@ -149,7 +151,33 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) { EXPECT_ONE_SOLUTION; } -// This test require a running rosmaster +// Using a Cartesian interpolation planner targeting a joint-space goal, which is +// transformed into a Cartesian goal by FK, should fail if the two poses are on different +// IK solution branches. In this case, the end-state, although reaching the Cartesian goal, +// will strongly deviate from the joint-space goal. +TEST(Panda, connectCartesianBranchesFails) { + Task t; + t.setRobotModel(loadModel()); + auto scene = std::make_shared(t.getRobotModel()); + scene->getCurrentStateNonConst().setToDefaultValues(); + scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready"); + t.add(std::make_unique("start", scene)); + + stages::Connect::GroupPlannerVector planner = { { "panda_arm", std::make_shared() } }; + t.add(std::make_unique("connect", planner)); + + // target an elbow-left instead of an elbow-right solution (different solution branch) + scene = scene->diff(); + scene->getCurrentStateNonConst().setJointGroupPositions( + "panda_arm", std::vector({ 2.72, 0.78, -2.63, -2.35, 0.36, 1.57, 0.48 })); + + t.add(std::make_unique("end", scene)); + EXPECT_FALSE(t.plan()); + EXPECT_STREQ(t.findChild("connect")->failures().front()->comment().c_str(), + "Trajectory end-point deviates too much from goal state"); +} + +// This test requires a running rosmaster TEST(Task, taskMoveConstructor) { auto create_task = [] { moveit::core::RobotModelConstPtr robot_model = getModel(); From 8f8f08b57b1e41d0f09d726bd81cc3ca9dcd71d7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 25 Feb 2024 13:47:58 +0100 Subject: [PATCH 8/8] fixup! simplify --- core/include/moveit/task_constructor/stages/connect.h | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index 9fce57445..bac878fd9 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -65,10 +65,6 @@ class Connect : public Connecting { protected: bool compatible(const InterfaceState& from_state, const InterfaceState& to_state) const override; - bool validateEndTrajectoryDeviation(const moveit::core::JointModelGroup* jmg, - const robot_trajectory::RobotTrajectoryPtr trajectory, - const moveit::core::RobotState& goal_state, double max_joint_deviation, - std::string& comment); public: enum MergeMode @@ -80,7 +76,7 @@ class Connect : public Connecting using GroupPlannerVector = std::vector >; Connect(const std::string& name = "connect", const GroupPlannerVector& planners = {}); - void setMaxJointDeviation(double max_joint_deviation) { setProperty("max_joint_deviation", max_joint_deviation); } + void setMaxDistance(double max_distance) { setProperty("max_distance", max_distance); } void setPathConstraints(moveit_msgs::Constraints path_constraints) { setProperty("path_constraints", std::move(path_constraints)); }