mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
allow arbitrary reference frame for target_pose and ik frame
This commit is contained in:
parent
4fcac84d76
commit
7531b8ad54
@ -18,6 +18,15 @@ MOVEIT_CLASS_FORWARD(JointModelGroup)
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
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 {
|
class ComputeIK : public WrapperBase {
|
||||||
public:
|
public:
|
||||||
ComputeIK(const std::string &name, pointer &&child = Stage::pointer());
|
ComputeIK(const std::string &name, pointer &&child = Stage::pointer());
|
||||||
@ -28,13 +37,27 @@ public:
|
|||||||
void setTimeout(double timeout);
|
void setTimeout(double timeout);
|
||||||
void setEndEffector(const std::string& eef);
|
void setEndEffector(const std::string& eef);
|
||||||
|
|
||||||
void setTargetPose(const geometry_msgs::PoseStamped &pose);
|
/// setters for reference frame
|
||||||
void setTargetPose(const Eigen::Affine3d& pose, const std::string& link = "");
|
void setReferenceFrame(const geometry_msgs::PoseStamped &pose);
|
||||||
|
void setReferenceFrame(const Eigen::Affine3d& pose, const std::string& frame = "");
|
||||||
template <typename T>
|
template <typename T>
|
||||||
void setTargetPose(const T& p, const std::string& link = "") {
|
void setReferenceFrame(const T& p, const std::string& frame = "") {
|
||||||
Eigen::Affine3d pose; pose = p;
|
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 <typename T>
|
||||||
|
void setTargetPose(const T& p, const std::string& frame = "") {
|
||||||
|
Eigen::Affine3d pose; pose = p;
|
||||||
|
setTargetPose(pose, frame);
|
||||||
|
}
|
||||||
|
|
||||||
void setMaxIKSolutions(uint32_t n);
|
void setMaxIKSolutions(uint32_t n);
|
||||||
void setIgnoreCollisions(bool flag);
|
void setIgnoreCollisions(bool flag);
|
||||||
};
|
};
|
||||||
|
|||||||
@ -30,8 +30,10 @@ ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child)
|
|||||||
p.declare<uint32_t>("max_ik_solutions", 1);
|
p.declare<uint32_t>("max_ik_solutions", 1);
|
||||||
p.declare<bool>("ignore_collisions", false);
|
p.declare<bool>("ignore_collisions", false);
|
||||||
|
|
||||||
// target_pose is read from the interface
|
// reference_frame and target_pose are read from the interface
|
||||||
p.declare<geometry_msgs::PoseStamped>("target_pose");
|
p.declare<geometry_msgs::PoseStamped>("reference_frame", "frame to be moved towards goal pose");
|
||||||
|
p.configureInitFrom(Stage::INTERFACE, {"reference_frame"});
|
||||||
|
p.declare<geometry_msgs::PoseStamped>("target_pose", "goal pose for reference frame");
|
||||||
p.configureInitFrom(Stage::INTERFACE, {"target_pose"});
|
p.configureInitFrom(Stage::INTERFACE, {"target_pose"});
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -43,20 +45,32 @@ void ComputeIK::setEndEffector(const std::string &eef){
|
|||||||
setProperty("eef", 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)
|
void ComputeIK::setTargetPose(const geometry_msgs::PoseStamped &pose)
|
||||||
{
|
{
|
||||||
setProperty("target_pose", 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;
|
geometry_msgs::PoseStamped pose_msg;
|
||||||
pose_msg.header.frame_id = link;
|
pose_msg.header.frame_id = frame;
|
||||||
tf::poseEigenToMsg(pose, pose_msg.pose);
|
tf::poseEigenToMsg(pose, pose_msg.pose);
|
||||||
setTargetPose(pose_msg);
|
setTargetPose(pose_msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void ComputeIK::setMaxIKSolutions(uint32_t n){
|
void ComputeIK::setMaxIKSolutions(uint32_t n){
|
||||||
setProperty("max_ik_solutions", n);
|
setProperty("max_ik_solutions", n);
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user