allow arbitrary reference frame for target_pose and ik frame

This commit is contained in:
Robert Haschke 2018-03-24 11:53:34 +01:00
parent 4fcac84d76
commit 7531b8ad54
2 changed files with 46 additions and 9 deletions

View File

@ -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);
}; };

View File

@ -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);
} }