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