diff --git a/core/src/stages/generate_place_pose.cpp b/core/src/stages/generate_place_pose.cpp index 3aae1132..28a8dc43 100644 --- a/core/src/stages/generate_place_pose.cpp +++ b/core/src/stages/generate_place_pose.cpp @@ -55,6 +55,7 @@ GeneratePlacePose::GeneratePlacePose(const std::string& name) : GeneratePose(nam auto& p = properties(); p.declare("object"); p.declare("ik_frame"); + p.declare("allow_z_flip", false, "allow placing objects upside down"); } void GeneratePlacePose::onNewSolution(const SolutionBase& s) { @@ -110,7 +111,7 @@ void GeneratePlacePose::compute() { // 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, 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 Eigen::Isometry3d object = nominal * Eigen::AngleAxisd(flip * M_PI, Eigen::Vector3d::UnitX()); for (uint i = 0; i < z_rotations; ++i) { @@ -138,20 +139,21 @@ void GeneratePlacePose::compute() { } }; + uint z_flips = props.get("allow_z_flip") ? 1 : 0; if (object->getShapes().size() == 1) { switch (object->getShapes()[0]->type) { case shapes::CYLINDER: - spawner(target_pose, 2); + spawner(target_pose, z_flips); return; case shapes::BOX: { // consider 180/90 degree rotations about z axis const double* dims = static_cast(*object->getShapes()[0]).size; - spawner(target_pose, 2, (std::abs(dims[0] - dims[1]) < 1e-5) ? 4 : 2); + spawner(target_pose, z_flips, (std::abs(dims[0] - dims[1]) < 1e-5) ? 4 : 2); return; } case shapes::SPHERE: // keep original orientation and rotate about world's z target_pose.linear() = orig_object_pose.linear(); - spawner(target_pose, 1); + spawner(target_pose, z_flips); return; default: break;