forward properties

This commit is contained in:
Robert Haschke 2018-04-08 16:45:12 +02:00
parent ce10d96c5c
commit abb68da083
2 changed files with 35 additions and 34 deletions

View File

@ -54,9 +54,13 @@ namespace moveit { namespace task_constructor { namespace stages {
* The wrapper reads a target_pose from the interface state of solutions provided * 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 * by the wrapped stage. This Cartesian pose (PoseStamped msg) is used as a goal
* pose for inverse kinematics. * pose for inverse kinematics.
*
* Usually, the end effector's parent link or the group's tip link is used as * Usually, the end effector's parent link or the group's tip link is used as
* the IK frame, which should be moved to the goal frame. However, any other * the IK frame, which should be moved to the goal frame. However, any other
* IK frame can be defined (which is linked to the tip of the group). * IK frame can be defined (which is linked to the tip of the group).
*
* Properties of the internally received InterfaceState can be forwarded to the
* newly generated, externally exposed InterfaceState.
*/ */
class ComputeIK : public WrapperBase { class ComputeIK : public WrapperBase {
public: public:
@ -65,11 +69,17 @@ public:
void init(const core::RobotModelConstPtr &robot_model); void init(const core::RobotModelConstPtr &robot_model);
void onNewSolution(const SolutionBase &s) override; void onNewSolution(const SolutionBase &s) override;
void setTimeout(double timeout); void setTimeout(double timeout) {
void setEndEffector(const std::string& eef); setProperty("timeout", timeout);
}
void setEndEffector(const std::string& eef) {
setProperty("eef", eef);
}
/// setters for IK frame /// setters for IK frame
void setIKFrame(const geometry_msgs::PoseStamped &pose); void setIKFrame(const geometry_msgs::PoseStamped &pose) {
setProperty("ik_frame", pose);
}
void setIKFrame(const Eigen::Affine3d& pose, const std::string& link); void setIKFrame(const Eigen::Affine3d& pose, const std::string& link);
template <typename T> template <typename T>
void setIKFrame(const T& p, const std::string& link) { void setIKFrame(const T& p, const std::string& link) {
@ -81,7 +91,9 @@ public:
} }
/// setters for target pose /// setters for target pose
void setTargetPose(const geometry_msgs::PoseStamped &pose); void setTargetPose(const geometry_msgs::PoseStamped &pose) {
setProperty("target_pose", pose);
}
void setTargetPose(const Eigen::Affine3d& pose, const std::string& frame = ""); void setTargetPose(const Eigen::Affine3d& pose, const std::string& frame = "");
template <typename T> template <typename T>
void setTargetPose(const T& p, const std::string& frame = "") { void setTargetPose(const T& p, const std::string& frame = "") {
@ -89,8 +101,16 @@ public:
setTargetPose(pose, frame); setTargetPose(pose, frame);
} }
void setMaxIKSolutions(uint32_t n); void setMaxIKSolutions(uint32_t n) {
void setIgnoreCollisions(bool flag); setProperty("max_ik_solutions", n);
}
void setIgnoreCollisions(bool flag) {
setProperty("ignore_collisions", flag);
}
void setForwardedProperties(const std::set<std::string>& names) {
setProperty("forward_properties", names);
}
}; };
} } } } } }

View File

@ -61,6 +61,7 @@ ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child)
p.declare<std::string>("default_pose", "", "default joint pose of active group (defines cost of IK)"); p.declare<std::string>("default_pose", "", "default joint pose of active group (defines cost of IK)");
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);
p.declare<std::set<std::string>>("forward_properties", "to-be-forwarded properties from input");
// ik_frame and target_pose are read from the interface // ik_frame and target_pose are read from the interface
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose"); p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
@ -68,19 +69,6 @@ ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child)
p.configureInitFrom(Stage::INTERFACE, {"target_pose"}); p.configureInitFrom(Stage::INTERFACE, {"target_pose"});
} }
void ComputeIK::setTimeout(double timeout){
setProperty("timeout", timeout);
}
void ComputeIK::setEndEffector(const std::string &eef){
setProperty("eef", eef);
}
void ComputeIK::setIKFrame(const geometry_msgs::PoseStamped &pose)
{
setProperty("ik_frame", pose);
}
void ComputeIK::setIKFrame(const Eigen::Affine3d &pose, const std::string &link) void ComputeIK::setIKFrame(const Eigen::Affine3d &pose, const std::string &link)
{ {
geometry_msgs::PoseStamped pose_msg; geometry_msgs::PoseStamped pose_msg;
@ -89,11 +77,6 @@ void ComputeIK::setIKFrame(const Eigen::Affine3d &pose, const std::string &link)
setIKFrame(pose_msg); setIKFrame(pose_msg);
} }
void ComputeIK::setTargetPose(const geometry_msgs::PoseStamped &pose)
{
setProperty("target_pose", pose);
}
void ComputeIK::setTargetPose(const Eigen::Affine3d &pose, const std::string &frame) void ComputeIK::setTargetPose(const Eigen::Affine3d &pose, const std::string &frame)
{ {
geometry_msgs::PoseStamped pose_msg; geometry_msgs::PoseStamped pose_msg;
@ -102,15 +85,6 @@ void ComputeIK::setTargetPose(const Eigen::Affine3d &pose, const std::string &fr
setTargetPose(pose_msg); setTargetPose(pose_msg);
} }
void ComputeIK::setMaxIKSolutions(uint32_t n){
setProperty("max_ik_solutions", n);
}
void ComputeIK::setIgnoreCollisions(bool flag)
{
setProperty("ignore_collisions", flag);
}
// found IK solutions with a flag indicating validity // found IK solutions with a flag indicating validity
typedef std::vector<std::vector<double>> IKSolutions; typedef std::vector<std::vector<double>> IKSolutions;
@ -394,7 +368,14 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
robot_state.setJointGroupPositions(jmg, ik_solutions.back().data()); robot_state.setJointGroupPositions(jmg, ik_solutions.back().data());
robot_state.update(); robot_state.update();
spawn(InterfaceState(scene), std::move(solution)); InterfaceState state(scene);
const boost::any &forwards = props.get("forward_properties");
if (!forwards.empty()) {
auto p = s.start()->properties();
p.exposeTo(state.properties(), boost::any_cast<std::set<std::string>>(forwards));
}
spawn(std::move(state), std::move(solution));
} }
// TODO: magic constant should be a property instead ("current_seed_only", or equivalent) // TODO: magic constant should be a property instead ("current_seed_only", or equivalent)