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.
This commit is contained in:
Robert Haschke 2022-08-27 17:09:02 +02:00
parent ec366b26ee
commit fd123cc4a7
9 changed files with 34 additions and 30 deletions

View File

@ -66,8 +66,8 @@ public:
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, 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,
robot_trajectory::RobotTrajectoryPtr& result, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
}; };
} // namespace solvers } // namespace solvers

View File

@ -63,8 +63,8 @@ public:
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout, const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
robot_trajectory::RobotTrajectoryPtr& result, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
}; };
} // namespace solvers } // namespace solvers

View File

@ -84,8 +84,8 @@ public:
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout, const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
robot_trajectory::RobotTrajectoryPtr& result, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
protected: protected:

View File

@ -84,9 +84,10 @@ public:
robot_trajectory::RobotTrajectoryPtr& result, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0; 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, 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, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0; const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
}; };

View File

@ -70,11 +70,13 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
} }
// reach pose of forward kinematics // 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, 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, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) { const moveit_msgs::Constraints& path_constraints) {
const auto& props = properties(); 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, &(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
moveit::core::MaxEEFStep(props.get<double>("step_size")), moveit::core::MaxEEFStep(props.get<double>("step_size")),
moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid, moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid,
props.get<kinematics::KinematicsQueryOptions>("kinematics_options")); props.get<kinematics::KinematicsQueryOptions>("kinematics_options"), offset);
assert(!trajectory.empty()); // there should be at least the start state assert(!trajectory.empty()); // there should be at least the start state
result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg); result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg);

View File

@ -99,9 +99,9 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
} }
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen, const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
const moveit::core::JointModelGroup* jmg, double timeout, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
robot_trajectory::RobotTrajectoryPtr& result, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) { const moveit_msgs::Constraints& path_constraints) {
const auto start_time = std::chrono::steady_clock::now(); 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()); 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 // 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 // in case of an invalid solution feedback should include unwanted collisions or violated constraints
ROS_WARN_NAMED("JointInterpolationPlanner", "IK failed for pose target"); ROS_WARN_NAMED("JointInterpolationPlanner", "IK failed for pose target");

View File

@ -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, bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target_eigen, const moveit::core::JointModelGroup* jmg, const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target_eigen,
double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) { const moveit_msgs::Constraints& path_constraints) {
const auto& props = properties(); const auto& props = properties();
moveit_msgs::MotionPlanRequest req; moveit_msgs::MotionPlanRequest req;
@ -181,7 +182,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co
geometry_msgs::PoseStamped target; geometry_msgs::PoseStamped target;
target.header.frame_id = from->getPlanningFrame(); 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.resize(1);
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(

View File

@ -273,22 +273,22 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
} }
COMPUTE: COMPUTE:
const Eigen::Isometry3d& link_pose = scene->getCurrentState().getGlobalLinkTransform(link); // offset from link to ik_frame
// transform target pose such that ik frame will reach there if link does const Eigen::Isometry3d& offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
target_eigen = target_eigen * ik_pose_world.inverse() * link_pose;
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(); robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
reached_state->updateLinkTransforms(); 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; double distance = 0.0;
if (use_rotation_distance) { 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(); distance = rotation.angle();
} else } else
distance = (reached_pose.translation() - link_pose.translation()).norm(); distance = (reached_pose.translation() - ik_pose_world.translation()).norm();
// min_distance reached? // min_distance reached?
if (min_distance > 0.0) { if (min_distance > 0.0) {
@ -306,8 +306,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
// visualize plan // visualize plan
auto ns = props.get<std::string>("marker_ns"); auto ns = props.get<std::string>("marker_ns");
if (!ns.empty() && linear_norm > 0) { // ensures that 'distance' is the norm of the reached distance 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, visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), ik_pose_world, reached_pose,
distance); linear, distance);
} }
} }

View File

@ -228,11 +228,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
add_frame(target, "target frame"); add_frame(target, "target frame");
add_frame(ik_pose_world, "ik frame"); add_frame(ik_pose_world, "ik frame");
// transform target pose such that ik frame will reach there if link does // offset from link to ik_frame
target = target * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link); Eigen::Isometry3d offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
// plan to Cartesian target // 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 // store result