GeneratePlacePose: add property 'allow_z_flip'

This commit is contained in:
Robert Haschke 2021-09-15 07:53:38 +02:00
parent 9630f4d789
commit 184fab8e0a

View File

@ -55,6 +55,7 @@ GeneratePlacePose::GeneratePlacePose(const std::string& name) : GeneratePose(nam
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<geometry_msgs::PoseStamped>("ik_frame");
p.declare<bool>("allow_z_flip", false, "allow placing objects upside down");
} }
void GeneratePlacePose::onNewSolution(const SolutionBase& s) { 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 // 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, &object_to_ik, 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());
for (uint i = 0; i < z_rotations; ++i) { for (uint i = 0; i < z_rotations; ++i) {
@ -138,20 +139,21 @@ void GeneratePlacePose::compute() {
} }
}; };
uint z_flips = props.get<bool>("allow_z_flip") ? 1 : 0;
if (object->getShapes().size() == 1) { if (object->getShapes().size() == 1) {
switch (object->getShapes()[0]->type) { switch (object->getShapes()[0]->type) {
case shapes::CYLINDER: case shapes::CYLINDER:
spawner(target_pose, 2); spawner(target_pose, z_flips);
return; return;
case shapes::BOX: { // consider 180/90 degree rotations about z axis case shapes::BOX: { // consider 180/90 degree rotations about z axis
const double* dims = static_cast<const shapes::Box&>(*object->getShapes()[0]).size; const double* dims = static_cast<const shapes::Box&>(*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; 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(); target_pose.linear() = orig_object_pose.linear();
spawner(target_pose, 1); spawner(target_pose, z_flips);
return; return;
default: default:
break; break;