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();
p.declare<std::string>("object");
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) {
@ -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<bool>("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<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;
}
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;