From 402d6a4bfedee9b12fc1397dc21b78e2d869b55c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 31 Oct 2022 16:10:59 +0100 Subject: [PATCH 1/6] Improve unittest for move_relative --- core/test/test_move_relative.cpp | 53 ++++++++++++++++++++------------ 1 file changed, 34 insertions(+), 19 deletions(-) diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp index a7f2d4d3..37c12705 100644 --- a/core/test/test_move_relative.cpp +++ b/core/test/test_move_relative.cpp @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -17,7 +18,7 @@ using namespace planning_scene; using namespace moveit::core; constexpr double TAU{ 2 * M_PI }; -constexpr double EPS{ 1e-6 }; +constexpr double EPS{ 5e-5 }; // provide a basic test fixture that prepares a Task struct PandaMoveRelative : public testing::Test @@ -66,10 +67,24 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) return aco; } -inline auto position(const PlanningSceneConstPtr& scene, const std::string& frame) { - return scene->getFrameTransform(frame).translation(); +void expect_const_position(const SolutionBaseConstPtr& solution, const std::string& tip, + const Eigen::Isometry3d& offset = Eigen::Isometry3d::Identity()) { + const robot_trajectory::RobotTrajectory& t = *std::dynamic_pointer_cast(solution)->trajectory(); + const Eigen::Vector3d start_position = (t.getFirstWayPoint().getFrameTransform(tip) * offset).translation(); + for (size_t i = 0; i < t.getWayPointCount(); ++i) { + const Eigen::Vector3d position = (t.getWayPoint(i).getFrameTransform(tip) * offset).translation(); + ASSERT_TRUE(start_position.isApprox(position, EPS)) + << "Rotation must maintain position\n" + << i << ": " << start_position.transpose() << " != " << position.transpose(); + } } +#define EXPECT_CONST_POSITION(...) \ + { \ + SCOPED_TRACE("expect_constant_position(" #__VA_ARGS__ ")"); \ + expect_const_position(__VA_ARGS__); \ + } + TEST_F(PandaMoveRelative, cartesianRotateEEF) { move->setDirection([] { geometry_msgs::TwistStamped twist; @@ -79,15 +94,22 @@ TEST_F(PandaMoveRelative, cartesianRotateEEF) { }()); ASSERT_TRUE(t.plan()) << "Failed to plan"; + EXPECT_CONST_POSITION(move->solutions().front(), group->getOnlyOneEndEffectorTip()->getName()); +} - const auto& tip_name{ group->getOnlyOneEndEffectorTip()->getName() }; - const auto start_eef_position{ position(scene, tip_name) }; - const auto end_eef_position{ position(move->solutions().front()->end()->scene(), tip_name) }; +TEST_F(PandaMoveRelative, cartesianCircular) { + const std::string tip = "panda_hand"; + auto offset = Eigen::Translation3d(0, 0, 0.1); + move->setIKFrame(offset, tip); + move->setDirection([] { + geometry_msgs::TwistStamped twist; + twist.header.frame_id = "world"; + twist.twist.angular.x = TAU / 4.0; + return twist; + }()); - EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS)) - << "Cartesian rotation unexpectedly changed position of '" << tip_name << "' (must only change orientation)\n" - << start_eef_position << "\nvs\n" - << end_eef_position; + ASSERT_TRUE(t.plan()) << "Failed to plan"; + EXPECT_CONST_POSITION(move->solutions().front(), tip, Eigen::Isometry3d(offset)); } TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) { @@ -102,15 +124,8 @@ TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) { return twist; }()); - ASSERT_TRUE(t.plan()); - - const auto start_eef_position{ position(scene, ATTACHED_OBJECT) }; - const auto end_eef_position{ position(move->solutions().front()->end()->scene(), ATTACHED_OBJECT) }; - - EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS)) - << "Cartesian rotation unexpectedly changed position of ik frame (must only change orientation)\n" - << start_eef_position << "\nvs\n" - << end_eef_position; + ASSERT_TRUE(t.plan()) << "Failed to plan"; + EXPECT_CONST_POSITION(move->solutions().front(), ATTACHED_OBJECT); } int main(int argc, char** argv) { From 076957d4dce896cbe5a9870b46b96292d84a6ff1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 26 Aug 2022 17:21:48 +0200 Subject: [PATCH 2/6] Simplify MoveRelative --- core/src/stages/move_relative.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index f9b2825f..f3435f2a 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -203,10 +203,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning Eigen::Vector3d linear; // linear translation Eigen::Vector3d angular; // angular rotation double linear_norm = 0.0, angular_norm = 0.0; - Eigen::Isometry3d target_eigen; - Eigen::Isometry3d link_pose = - scene->getCurrentState().getGlobalLinkTransform(link); // take a copy here, pose will change on success try { // try to extract Twist const geometry_msgs::TwistStamped& target = boost::any_cast(direction); @@ -277,8 +274,9 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning } COMPUTE: + const Eigen::Isometry3d& link_pose = scene->getCurrentState().getGlobalLinkTransform(link); // transform target pose such that ik frame will reach there if link does - target_eigen = target_eigen * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link); + target_eigen = target_eigen * ik_pose_world.inverse() * link_pose; success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); From ec366b26ee095fca2ce6c7a80c72dbcc7907b128 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 26 Aug 2022 22:28:54 +0200 Subject: [PATCH 3/6] MoveRelative: Correctly compute motion transform The twist motion performs an angular rotation about the given axis _and_ the origin of ik_frame as well as a linear translation. Both transforms are expressed w.r.t. the model frame and thus require left-multiplication to ik_frame's current pose. --- core/src/stages/move_relative.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index f3435f2a..16b7736d 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -237,13 +237,13 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning angular *= -1.0; } - // compute absolute transform for link + // compute target transform for ik_frame applying motion transform of twist + // linear+angular are expressed w.r.t. model frame and thus we need left-multiplication linear = frame_pose.linear() * linear; angular = frame_pose.linear() * angular; - target_eigen = ik_pose_world; - target_eigen.linear() = - target_eigen.linear() * Eigen::AngleAxisd(angular_norm, ik_pose_world.linear().transpose() * angular); - target_eigen.translation() += linear; + auto R = Eigen::AngleAxisd(angular_norm, angular); + auto p = ik_pose_world.translation(); + target_eigen = Eigen::Translation3d(linear + p - R * p) * (R * ik_pose_world); goto COMPUTE; } catch (const boost::bad_any_cast&) { /* continue with Vector */ } @@ -264,10 +264,9 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning if (dir == Interface::BACKWARD) linear *= -1.0; - // compute absolute transform for link + // compute target transform for ik_frame applying delta transform of twist linear = frame_pose.linear() * linear; - target_eigen = ik_pose_world; - target_eigen.translation() += linear; + target_eigen = Eigen::Translation3d(linear) * ik_pose_world; } catch (const boost::bad_any_cast&) { solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name()); return false; From fd123cc4a77a42a88def59392cb81fa3dc938c3b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 27 Aug 2022 17:09:02 +0200 Subject: [PATCH 4/6] Fix handling of ik_frame in Cartesian path planning The ik_frame should move in a straight-line Cartesian path. However, so far the link frame was following a Cartesian path. --- .../task_constructor/solvers/cartesian_path.h | 4 ++-- .../solvers/joint_interpolation.h | 4 ++-- .../solvers/pipeline_planner.h | 4 ++-- .../solvers/planner_interface.h | 5 +++-- core/src/solvers/cartesian_path.cpp | 8 +++++--- core/src/solvers/joint_interpolation.cpp | 8 ++++---- core/src/solvers/pipeline_planner.cpp | 7 ++++--- core/src/stages/move_relative.cpp | 18 +++++++++--------- core/src/stages/move_to.cpp | 6 +++--- 9 files changed, 34 insertions(+), 30 deletions(-) diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h index 72e63fe3..732111cb 100644 --- a/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -66,8 +66,8 @@ public: const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; }; } // namespace solvers diff --git a/core/include/moveit/task_constructor/solvers/joint_interpolation.h b/core/include/moveit/task_constructor/solvers/joint_interpolation.h index 6f27c240..ae909120 100644 --- a/core/include/moveit/task_constructor/solvers/joint_interpolation.h +++ b/core/include/moveit/task_constructor/solvers/joint_interpolation.h @@ -63,8 +63,8 @@ public: const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; }; } // namespace solvers diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h index 75af734b..3e072e52 100644 --- a/core/include/moveit/task_constructor/solvers/pipeline_planner.h +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -84,8 +84,8 @@ public: const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; protected: diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h index 65a7d3ac..60139b3b 100644 --- a/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -84,9 +84,10 @@ public: robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0; - /// plan trajectory from current robot state to Cartesian target + /// plan trajectory from current robot state to Cartesian target, such that pose(link)*offset == target virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0; }; diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 2e5fac02..9852470f 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -70,11 +70,13 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, } // reach pose of forward kinematics - return plan(from, *link, to->getCurrentState().getGlobalLinkTransform(link), jmg, timeout, result, path_constraints); + return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg, + timeout, result, path_constraints); } bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double /*timeout*/, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double /*timeout*/, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) { const auto& props = properties(); @@ -96,7 +98,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons &(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true, moveit::core::MaxEEFStep(props.get("step_size")), moveit::core::JumpThreshold(props.get("jump_threshold")), is_valid, - props.get("kinematics_options")); + props.get("kinematics_options"), offset); assert(!trajectory.empty()); // there should be at least the start state result = std::make_shared(sandbox_scene->getRobotModel(), jmg); diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index fc687613..600fd6f0 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -99,9 +99,9 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr } bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, - const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen, - const moveit::core::JointModelGroup* jmg, double timeout, - robot_trajectory::RobotTrajectoryPtr& result, + const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset, + const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) { const auto start_time = std::chrono::steady_clock::now(); @@ -117,7 +117,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr return to->isStateValid(*robot_state, constraints, jmg->getName()); } }; - if (!to->getCurrentStateNonConst().setFromIK(jmg, target_eigen, link.getName(), timeout, is_valid)) { + if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid)) { // TODO(v4hn): planners need a way to add feedback to failing plans // in case of an invalid solution feedback should include unwanted collisions or violated constraints ROS_WARN_NAMED("JointInterpolationPlanner", "IK failed for pose target"); diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index d2d16295..66d12d5c 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -172,8 +172,9 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, } bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, - const Eigen::Isometry3d& target_eigen, const moveit::core::JointModelGroup* jmg, - double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target_eigen, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) { const auto& props = properties(); moveit_msgs::MotionPlanRequest req; @@ -181,7 +182,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co geometry_msgs::PoseStamped target; target.header.frame_id = from->getPlanningFrame(); - target.pose = tf2::toMsg(target_eigen); + target.pose = tf2::toMsg(target_eigen * offset.inverse()); req.goal_constraints.resize(1); req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 16b7736d..0489445b 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -273,22 +273,22 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning } COMPUTE: - const Eigen::Isometry3d& link_pose = scene->getCurrentState().getGlobalLinkTransform(link); - // transform target pose such that ik frame will reach there if link does - target_eigen = target_eigen * ik_pose_world.inverse() * link_pose; + // offset from link to ik_frame + const Eigen::Isometry3d& offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; - success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); + success = + planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints); robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr(); reached_state->updateLinkTransforms(); - const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link); + const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset; double distance = 0.0; if (use_rotation_distance) { - Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose()); + Eigen::AngleAxisd rotation(reached_pose.linear() * ik_pose_world.linear().transpose()); distance = rotation.angle(); } else - distance = (reached_pose.translation() - link_pose.translation()).norm(); + distance = (reached_pose.translation() - ik_pose_world.translation()).norm(); // min_distance reached? if (min_distance > 0.0) { @@ -306,8 +306,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning // visualize plan auto ns = props.get("marker_ns"); if (!ns.empty() && linear_norm > 0) { // ensures that 'distance' is the norm of the reached distance - visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), link_pose, reached_pose, linear, - distance); + visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), ik_pose_world, reached_pose, + linear, distance); } } diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 5611475c..0ce8ea11 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -228,11 +228,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP add_frame(target, "target frame"); add_frame(ik_pose_world, "ik frame"); - // transform target pose such that ik frame will reach there if link does - target = target * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link); + // offset from link to ik_frame + Eigen::Isometry3d offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; // plan to Cartesian target - success = planner_->plan(state.scene(), *link, target, jmg, timeout, robot_trajectory, path_constraints); + success = planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints); } // store result From 32d3454c1fdaa698e5c96340ca4b68f9aa67ad27 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 28 Aug 2022 10:22:37 +0200 Subject: [PATCH 5/6] Fix getRobotTipForFrame() When passing the root frame, getRigidlyConnectedParentLinkModel() returns a nullptr for robot_link, causing a segfault. Actually, we don't need to use that method at all. We just need to find the robot_link of an associated body. --- core/src/utils.cpp | 30 +++++++++++++++++------------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/core/src/utils.cpp b/core/src/utils.cpp index 8b7c484b..37669277 100644 --- a/core/src/utils.cpp +++ b/core/src/utils.cpp @@ -69,23 +69,27 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link); } else { auto ik_pose_msg = boost::any_cast(property.value()); - if (ik_pose_msg.header.frame_id.empty()) { - if (!(robot_link = get_tip())) { - solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found"); - return false; - } - tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); - tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link) * tip_in_global_frame; - } else if (scene.knowsFrameTransform(ik_pose_msg.header.frame_id)) { - robot_link = scene.getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id); - tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); - tip_in_global_frame = scene.getFrameTransform(ik_pose_msg.header.frame_id) * tip_in_global_frame; - } else { + tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); + + robot_link = nullptr; + bool found = false; + auto ref_frame = scene.getCurrentState().getFrameInfo(ik_pose_msg.header.frame_id, robot_link, found); + if (!found && !ik_pose_msg.header.frame_id.empty()) { std::stringstream ss; - ss << "ik_frame specified in unknown frame '" << ik_pose_msg << "'"; + ss << "ik_frame specified in unknown frame '" << ik_pose_msg.header.frame_id << "'"; solution.markAsFailure(ss.str()); return false; } + if (!robot_link) + robot_link = get_tip(); + if (!robot_link) { + solution.markAsFailure("ik_frame doesn't specify a link frame"); + return false; + } else if (!found) { // use robot link's frame as reference by default + ref_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link); + } + + tip_in_global_frame = ref_frame * tip_in_global_frame; } return true; From d66b2262edd3f05429c6e49068c547bdac903ebb Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 28 Aug 2022 11:13:23 +0200 Subject: [PATCH 6/6] Rename variables in visualizePlan() - link_pose -> start_pose - pos_link -> pos_start --- core/src/stages/move_relative.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 0489445b..30af9aa5 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -104,8 +104,9 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c return false; } +// Create an arrow marker from start_pose to reached_pose, split into a red and green part based on achieved distance static void visualizePlan(std::deque& markers, Interface::Direction dir, bool success, - const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& link_pose, + const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& reached_pose, const Eigen::Vector3d& linear, double distance) { double linear_norm = linear.norm(); @@ -114,7 +115,7 @@ static void visualizePlan(std::deque& markers, Inter auto quat_cylinder = quat_target * Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY()); // link position before planning; reached link position after planning; target link position - Eigen::Vector3d pos_link = link_pose.translation(); + Eigen::Vector3d pos_start = start_pose.translation(); Eigen::Vector3d pos_reached = reached_pose.translation(); Eigen::Vector3d pos_target = pos_reached + quat_target * Eigen::Vector3d(linear_norm - distance, 0, 0); @@ -124,7 +125,7 @@ static void visualizePlan(std::deque& markers, Inter if (dir == Interface::FORWARD) { if (success) { // valid part: green arrow - rviz_marker_tools::makeArrow(m, pos_link, pos_reached, 0.1 * linear_norm); + rviz_marker_tools::makeArrow(m, pos_start, pos_reached, 0.1 * linear_norm); rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN); markers.push_back(m); } else { @@ -139,14 +140,14 @@ static void visualizePlan(std::deque& markers, Inter rviz_marker_tools::makeCylinder(m, 0.1 * linear_norm, distance); rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN); // position half-way between pos_link and pos_reached - m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_link + pos_reached) }); + m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_start + pos_reached) }); m.pose.orientation = tf2::toMsg(quat_cylinder); markers.push_back(m); } } else { // valid part: green arrow // head length according to above comment - rviz_marker_tools::makeArrow(m, pos_reached, pos_link, 0.1 * linear_norm, 0.23 * linear_norm); + rviz_marker_tools::makeArrow(m, pos_reached, pos_start, 0.1 * linear_norm, 0.23 * linear_norm); rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN); markers.push_back(m); if (!success) {