mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
CartesianPath: allow ik_frame definition
... if start and end are given as joint-space poses
This commit is contained in:
parent
177e19de1f
commit
b1336dc210
@ -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); }
|
||||
|
||||
@ -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,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user