diff --git a/core/include/moveit/task_constructor/stages/compute_ik.h b/core/include/moveit/task_constructor/stages/compute_ik.h index e442be81..fea8a909 100644 --- a/core/include/moveit/task_constructor/stages/compute_ik.h +++ b/core/include/moveit/task_constructor/stages/compute_ik.h @@ -18,6 +18,15 @@ MOVEIT_CLASS_FORWARD(JointModelGroup) namespace moveit { namespace task_constructor { namespace stages { +/** Wrapper for any pose generator stage to compute IK poses for a Cartesian pose. + * + * The wrapper reads a target_pose from the interface state of solutions provided + * by the wrapped stage. This Cartesian pose (PoseStamped msg) is used as a goal + * pose for inverse kinematics. + * Usually, the end effector's parent link or the group's tip link is used as + * the reference frame, which should be moved to the goal frame. However, any other + * reference frame can be defined (which is linked to the tip of the group). + */ class ComputeIK : public WrapperBase { public: ComputeIK(const std::string &name, pointer &&child = Stage::pointer()); @@ -28,13 +37,27 @@ public: void setTimeout(double timeout); void setEndEffector(const std::string& eef); - void setTargetPose(const geometry_msgs::PoseStamped &pose); - void setTargetPose(const Eigen::Affine3d& pose, const std::string& link = ""); + /// setters for reference frame + void setReferenceFrame(const geometry_msgs::PoseStamped &pose); + void setReferenceFrame(const Eigen::Affine3d& pose, const std::string& frame = ""); template - void setTargetPose(const T& p, const std::string& link = "") { + void setReferenceFrame(const T& p, const std::string& frame = "") { Eigen::Affine3d pose; pose = p; - setTargetPose(pose, link); + setTargetPose(pose, frame); } + void setReferenceFrame(const std::string& frame) { + setTargetPose(Eigen::Affine3d::Identity(), frame); + } + + /// setters for target pose + void setTargetPose(const geometry_msgs::PoseStamped &pose); + void setTargetPose(const Eigen::Affine3d& pose, const std::string& frame = ""); + template + void setTargetPose(const T& p, const std::string& frame = "") { + Eigen::Affine3d pose; pose = p; + setTargetPose(pose, frame); + } + void setMaxIKSolutions(uint32_t n); void setIgnoreCollisions(bool flag); }; diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index f04d2ce9..40342f72 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -30,8 +30,10 @@ ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child) p.declare("max_ik_solutions", 1); p.declare("ignore_collisions", false); - // target_pose is read from the interface - p.declare("target_pose"); + // reference_frame and target_pose are read from the interface + p.declare("reference_frame", "frame to be moved towards goal pose"); + p.configureInitFrom(Stage::INTERFACE, {"reference_frame"}); + p.declare("target_pose", "goal pose for reference frame"); p.configureInitFrom(Stage::INTERFACE, {"target_pose"}); } @@ -43,20 +45,32 @@ void ComputeIK::setEndEffector(const std::string &eef){ setProperty("eef", eef); } +void ComputeIK::setReferenceFrame(const geometry_msgs::PoseStamped &pose) +{ + setProperty("reference_frame", pose); +} + +void ComputeIK::setReferenceFrame(const Eigen::Affine3d &pose, const std::string &frame) +{ + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.frame_id = frame; + tf::poseEigenToMsg(pose, pose_msg.pose); + setReferenceFrame(pose_msg); +} + void ComputeIK::setTargetPose(const geometry_msgs::PoseStamped &pose) { setProperty("target_pose", pose); } -void ComputeIK::setTargetPose(const Eigen::Affine3d &pose, const std::string &link) +void ComputeIK::setTargetPose(const Eigen::Affine3d &pose, const std::string &frame) { geometry_msgs::PoseStamped pose_msg; - pose_msg.header.frame_id = link; + pose_msg.header.frame_id = frame; tf::poseEigenToMsg(pose, pose_msg.pose); setTargetPose(pose_msg); } - void ComputeIK::setMaxIKSolutions(uint32_t n){ setProperty("max_ik_solutions", n); }