mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
GeneratePlacePose: Remove property "ik_frame" from stage
- Instead, set ik_frame property on solution from passed object (frame). - Allow subframes to be used as "object" frames
This commit is contained in:
parent
0dffefd6ae
commit
d90b566b08
@ -54,7 +54,6 @@ namespace stages {
|
|||||||
GeneratePlacePose::GeneratePlacePose(const std::string& name) : GeneratePose(name) {
|
GeneratePlacePose::GeneratePlacePose(const std::string& name) : GeneratePose(name) {
|
||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
p.declare<std::string>("object");
|
p.declare<std::string>("object");
|
||||||
p.declare<geometry_msgs::PoseStamped>("ik_frame");
|
|
||||||
p.declare<bool>("allow_z_flip", false, "allow placing objects upside down");
|
p.declare<bool>("allow_z_flip", false, "allow placing objects upside down");
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -63,11 +62,14 @@ void GeneratePlacePose::onNewSolution(const SolutionBase& s) {
|
|||||||
|
|
||||||
const auto& props = properties();
|
const auto& props = properties();
|
||||||
const std::string& object = props.get<std::string>("object");
|
const std::string& object = props.get<std::string>("object");
|
||||||
|
bool frame_found = false;
|
||||||
|
const moveit::core::LinkModel* link = nullptr;
|
||||||
|
scene->getCurrentState().getFrameInfo(object, link, frame_found);
|
||||||
std::string msg;
|
std::string msg;
|
||||||
if (!scene->getCurrentState().hasAttachedBody(object))
|
if (!frame_found)
|
||||||
msg = "'" + object + "' is not an attached object";
|
msg = "frame '" + object + "' is not known";
|
||||||
if (scene->getCurrentState().getAttachedBody(object)->getShapes().empty())
|
if (!link)
|
||||||
msg = "'" + object + "' has no associated shapes";
|
msg = "frame '" + object + "' is not attached to the robot";
|
||||||
if (!msg.empty()) {
|
if (!msg.empty()) {
|
||||||
if (storeFailures()) {
|
if (storeFailures()) {
|
||||||
InterfaceState state(scene);
|
InterfaceState state(scene);
|
||||||
@ -92,25 +94,20 @@ void GeneratePlacePose::compute() {
|
|||||||
const moveit::core::RobotState& robot_state = scene->getCurrentState();
|
const moveit::core::RobotState& robot_state = scene->getCurrentState();
|
||||||
const auto& props = properties();
|
const auto& props = properties();
|
||||||
|
|
||||||
const moveit::core::AttachedBody* object = robot_state.getAttachedBody(props.get<std::string>("object"));
|
const std::string& frame_id = props.get<std::string>("object");
|
||||||
// current object_pose w.r.t. planning frame
|
geometry_msgs::PoseStamped ik_frame;
|
||||||
const Eigen::Isometry3d& orig_object_pose = object->getGlobalCollisionBodyTransforms()[0];
|
ik_frame.header.frame_id = frame_id;
|
||||||
|
ik_frame.pose = tf2::toMsg(Eigen::Isometry3d::Identity());
|
||||||
|
|
||||||
|
const moveit::core::AttachedBody* object = robot_state.getAttachedBody(frame_id);
|
||||||
const geometry_msgs::PoseStamped& pose_msg = props.get<geometry_msgs::PoseStamped>("pose");
|
const geometry_msgs::PoseStamped& pose_msg = props.get<geometry_msgs::PoseStamped>("pose");
|
||||||
Eigen::Isometry3d target_pose;
|
Eigen::Isometry3d target_pose;
|
||||||
tf2::fromMsg(pose_msg.pose, target_pose);
|
tf2::fromMsg(pose_msg.pose, target_pose);
|
||||||
// target pose w.r.t. planning frame
|
// target pose w.r.t. planning frame
|
||||||
scene->getTransforms().transformPose(pose_msg.header.frame_id, target_pose, target_pose);
|
scene->getTransforms().transformPose(pose_msg.header.frame_id, target_pose, target_pose);
|
||||||
|
|
||||||
const geometry_msgs::PoseStamped& ik_frame_msg = props.get<geometry_msgs::PoseStamped>("ik_frame");
|
|
||||||
Eigen::Isometry3d ik_frame;
|
|
||||||
tf2::fromMsg(ik_frame_msg.pose, ik_frame);
|
|
||||||
ik_frame = robot_state.getGlobalLinkTransform(ik_frame_msg.header.frame_id) * ik_frame;
|
|
||||||
Eigen::Isometry3d object_to_ik = orig_object_pose.inverse() * ik_frame;
|
|
||||||
|
|
||||||
// spawn the nominal target object pose, considering flip about z and rotations about z-axis
|
// spawn the nominal target object pose, considering flip about z and rotations about z-axis
|
||||||
auto spawner = [&s, &scene, &object_to_ik, this](const Eigen::Isometry3d& nominal, uint z_flips,
|
auto spawner = [&s, &scene, &ik_frame, this](const Eigen::Isometry3d& nominal, uint z_flips, uint z_rotations = 10) {
|
||||||
uint z_rotations = 10) {
|
|
||||||
for (uint flip = 0; flip <= z_flips; ++flip) {
|
for (uint flip = 0; flip <= z_flips; ++flip) {
|
||||||
// flip about object's x-axis
|
// flip about object's x-axis
|
||||||
Eigen::Isometry3d object = nominal * Eigen::AngleAxisd(flip * M_PI, Eigen::Vector3d::UnitX());
|
Eigen::Isometry3d object = nominal * Eigen::AngleAxisd(flip * M_PI, Eigen::Vector3d::UnitX());
|
||||||
@ -124,11 +121,12 @@ void GeneratePlacePose::compute() {
|
|||||||
// target ik_frame's pose w.r.t. planning frame
|
// target ik_frame's pose w.r.t. planning frame
|
||||||
geometry_msgs::PoseStamped target_pose_msg;
|
geometry_msgs::PoseStamped target_pose_msg;
|
||||||
target_pose_msg.header.frame_id = scene->getPlanningFrame();
|
target_pose_msg.header.frame_id = scene->getPlanningFrame();
|
||||||
target_pose_msg.pose = tf2::toMsg(object * object_to_ik);
|
target_pose_msg.pose = tf2::toMsg(object);
|
||||||
|
|
||||||
InterfaceState state(scene);
|
InterfaceState state(scene);
|
||||||
forwardProperties(*s.end(), state); // forward properties from inner solutions
|
forwardProperties(*s.end(), state); // forward properties from inner solutions
|
||||||
state.properties().set("target_pose", target_pose_msg);
|
state.properties().set("target_pose", target_pose_msg);
|
||||||
|
state.properties().set("ik_frame", ik_frame);
|
||||||
|
|
||||||
SubTrajectory trajectory;
|
SubTrajectory trajectory;
|
||||||
trajectory.setCost(0.0);
|
trajectory.setCost(0.0);
|
||||||
@ -140,7 +138,7 @@ void GeneratePlacePose::compute() {
|
|||||||
};
|
};
|
||||||
|
|
||||||
uint z_flips = props.get<bool>("allow_z_flip") ? 1 : 0;
|
uint z_flips = props.get<bool>("allow_z_flip") ? 1 : 0;
|
||||||
if (object->getShapes().size() == 1) {
|
if (object && object->getShapes().size() == 1) {
|
||||||
switch (object->getShapes()[0]->type) {
|
switch (object->getShapes()[0]->type) {
|
||||||
case shapes::CYLINDER:
|
case shapes::CYLINDER:
|
||||||
spawner(target_pose, z_flips);
|
spawner(target_pose, z_flips);
|
||||||
@ -152,7 +150,6 @@ void GeneratePlacePose::compute() {
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
case shapes::SPHERE: // keep original orientation and rotate about world's z
|
case shapes::SPHERE: // keep original orientation and rotate about world's z
|
||||||
target_pose.linear() = orig_object_pose.linear();
|
|
||||||
spawner(target_pose, z_flips);
|
spawner(target_pose, z_flips);
|
||||||
return;
|
return;
|
||||||
default:
|
default:
|
||||||
|
|||||||
@ -95,13 +95,9 @@ place_generator = stages.GeneratePlacePose("Generate Place Pose")
|
|||||||
place_generator.setMonitoredStage(task["Pick"])
|
place_generator.setMonitoredStage(task["Pick"])
|
||||||
place_generator.object = object_name
|
place_generator.object = object_name
|
||||||
place_generator.pose = placePose
|
place_generator.pose = placePose
|
||||||
place_generator.properties.configureInitFrom(
|
|
||||||
core.Stage.PropertyInitializerSource.PARENT, ["ik_frame"]
|
|
||||||
)
|
|
||||||
|
|
||||||
# The SimpleUnGrasp container encapsulates releasing the object at the given Cartesian pose
|
# The SimpleUnGrasp container encapsulates releasing the object at the given Cartesian pose
|
||||||
simpleUnGrasp = stages.SimpleUnGrasp(place_generator, "UnGrasp")
|
simpleUnGrasp = stages.SimpleUnGrasp(place_generator, "UnGrasp")
|
||||||
simpleUnGrasp.setIKFrame(ik_frame)
|
|
||||||
simpleUnGrasp.pregrasp = "open"
|
simpleUnGrasp.pregrasp = "open"
|
||||||
|
|
||||||
# Place container comprises placing, ungrasping, and retracting
|
# Place container comprises placing, ungrasping, and retracting
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user