mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
forward properties
This commit is contained in:
parent
ce10d96c5c
commit
abb68da083
@ -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);
|
||||||
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -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)
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user