CartesianPath: allow ik_frame definition

... if start and end are given as joint-space poses
This commit is contained in:
Robert Haschke 2024-06-29 17:58:29 +02:00
parent 177e19de1f
commit b1336dc210
2 changed files with 25 additions and 5 deletions

View File

@ -52,6 +52,10 @@ class CartesianPath : public PlannerInterface
public:
CartesianPath();
void setIKFrame(const geometry_msgs::PoseStamped& pose) { setProperty("ik_frame", pose); }
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
void setStepSize(double step_size) { setProperty("step_size", step_size); }
void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); }
void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); }

View File

@ -37,10 +37,12 @@
*/
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/utils.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/time_parameterization.h>
#include <moveit/kinematics_base/kinematics_base.h>
#include <moveit/robot_state/cartesian_interpolator.h>
#include <tf2_eigen/tf2_eigen.h>
using namespace trajectory_processing;
@ -50,6 +52,7 @@ namespace solvers {
CartesianPath::CartesianPath() {
auto& p = properties();
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to move linearly (use for joint-space target)");
p.declare<double>("step_size", 0.01, "step size between consecutive waypoints");
p.declare<double>("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step");
p.declare<double>("min_fraction", 1.0, "fraction of motion required for success");
@ -59,18 +62,31 @@ CartesianPath::CartesianPath() {
void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {}
void CartesianPath::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link;
pose_msg.pose = tf2::toMsg(pose);
setIKFrame(pose_msg);
}
PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
if (!link)
return { false, "no unique tip for joint model group: " + jmg->getName() };
const auto& props = properties();
const moveit::core::LinkModel* link;
std::string error_msg;
Eigen::Isometry3d ik_pose_world;
if (!utils::getRobotTipForFrame(props.property("ik_frame"), *from, jmg, error_msg, link, ik_pose_world))
return { false, error_msg };
Eigen::Isometry3d offset = from->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
// reach pose of forward kinematics
return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg,
std::min(timeout, properties().get<double>("timeout")), result, path_constraints);
return plan(from, *link, offset, to->getCurrentState().getGlobalLinkTransform(link), jmg,
std::min(timeout, props.get<double>("timeout")), result, path_constraints);
}
PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,